From 803d36f10ce8d18f1afb99a0ecb35128ca3c6908 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 10 May 2016 11:03:58 +0200 Subject: [PATCH 001/255] Some minor changes --- .../src/extract_surfel_features.cpp | 2 -- .../retrieval_processing/launch/processing.launch | 12 ++++++++---- .../retrieval_processing/src/retrieval_features.cpp | 3 +++ .../src/retrieval_simulate_observations.cpp | 4 ++++ 4 files changed, 15 insertions(+), 6 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/src/extract_surfel_features.cpp b/dynamic_object_retrieval/dynamic_object_retrieval/src/extract_surfel_features.cpp index 9f6c2799..31d947a8 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/src/extract_surfel_features.cpp +++ b/dynamic_object_retrieval/dynamic_object_retrieval/src/extract_surfel_features.cpp @@ -144,8 +144,6 @@ void compute_features(HistCloudT::Ptr& features, CloudT::Ptr& keypoints, CloudT: for (size_t i = 0; i < pfhrgb_cloud.size(); ++i) { std::copy(pfhrgb_cloud.at(i).histogram, pfhrgb_cloud.at(i).histogram+N, features->at(i).histogram); } - - std::cout << "Number of features: " << pfhrgb_cloud.size() << std::endl; } NormalCloudT::Ptr compute_surfel_normals(SurfelCloudT::Ptr& surfel_cloud, CloudT::Ptr& segment) diff --git a/quasimodo/retrieval_processing/launch/processing.launch b/quasimodo/retrieval_processing/launch/processing.launch index 13a50de0..373282b4 100644 --- a/quasimodo/retrieval_processing/launch/processing.launch +++ b/quasimodo/retrieval_processing/launch/processing.launch @@ -1,26 +1,30 @@ + + + + - + - + - + - + diff --git a/quasimodo/retrieval_processing/src/retrieval_features.cpp b/quasimodo/retrieval_processing/src/retrieval_features.cpp index 11d7a03d..6a49aa20 100644 --- a/quasimodo/retrieval_processing/src/retrieval_features.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_features.cpp @@ -157,6 +157,9 @@ void features_callback(const std_msgs::String::ConstPtr& msg) HistCloudT::Ptr desc_cloud(new HistCloudT); CloudT::Ptr kp_cloud(new CloudT); dynamic_object_retrieval::compute_features(desc_cloud, kp_cloud, segment, surfel_map); + + cout << "Saving " << feature_path.string() << " with " << desc_cloud->size() << " number of features. " << endl; + //test_compute_features(desc_cloud, kp_cloud, segment, surfel_map); if (desc_cloud->empty()) { // push back one inf point on descriptors and keypoints diff --git a/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp b/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp index 4dc24ad2..8e6ae444 100644 --- a/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp @@ -106,9 +106,11 @@ int main(int argc, char** argv) if (bypass_surfelize) { + cout << "Advertising " << "/surfelization_done" << endl; string_pub = n.advertise("/surfelization_done", 1); } else { + cout << "Advertising " << "/local_metric_map/room_observations" << endl; room_pub = n.advertise("/local_metric_map/room_observations", 1); } ros::Subscriber sub = n.subscribe("/vocabulary_done", 1, callback); @@ -116,11 +118,13 @@ int main(int argc, char** argv) ros::Duration(1.0).sleep(); if (bypass_surfelize) { + cout << "Publishing initial simulated observation after surfelization: " << sweep_xmls[sweep_ind] << endl; std_msgs::String sweep_msg; sweep_msg.data = sweep_xmls[sweep_ind++]; string_pub.publish(sweep_msg); } else { + cout << "Publishing initial simulated observation: " << sweep_xmls[sweep_ind] << endl; semantic_map::RoomObservation sweep_msg; sweep_msg.xml_file_name = sweep_xmls[sweep_ind++]; room_pub.publish(sweep_msg); From 7d561958ea2546442388a0a0ca76870ea1e3f31e Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Thu, 16 Jun 2016 14:33:15 +0200 Subject: [PATCH 002/255] Somewhat complete integration of object search, still need to change the retrieval package to take this into account --- .../dynamic_object_retrieval/summary_types.h | 69 +++++++++++++++++++ quasimodo/quasimodo_msgs/CMakeLists.txt | 3 + .../src/quasimodo_logging_server.cpp | 20 ++++-- .../src/quasimodo_retrieval_node.cpp | 2 + .../src/retrieval_features.cpp | 37 ++++++++++ .../src/retrieval_vocabulary.cpp | 65 ++++++++++++++++- 6 files changed, 189 insertions(+), 7 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h index f60ac110..dcd507e7 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h @@ -235,6 +235,75 @@ struct sweep_summary { sweep_summary() : nbr_segments(0), segment_indices(), segment_annotations() {} }; +struct segment_uris { + + //std::string noise_data_path; + std::vector uris; + + void load(const boost::filesystem::path& data_path) + { + std::ifstream in((data_path / boost::filesystem::path(vocabulary) / boost::filesystem::path("segment_uris.json")).string()); + { + cereal::JSONInputArchive archive_i(in); + archive_i(*this); + } + /* + if (!boost::filesystem::path(noise_data_path).is_absolute()) { + noise_data_path = boost::filesystem::canonical(noise_data_path, data_path.parent_path()).string(); + } + */ + + if (!uris.empty() && !boost::filesystem::path(index_convex_segment_paths[0]).is_absolute()) { + for (std::string& segment_path : uris) { + //segment_path = boost::filesystem::canonical(segment_path, data_path).string(); + //HMMM, this will start with a slash? + if (segment_path.compare(0, 7, "file://") == 0) { + segment_path = std::string("file://") + boost::filesystem::absolute(segment_path.substr(7, segment_path.size()-7), data_path.parent_path()).string(); + } + } + } + } + + void save(const boost::filesystem::path& data_path) + { + // we should just have some way of checking if the paths are childs of data_path, in that case replace with relative path + // but this is in general not true for this data_path, which is the vocabulary path! + // so, instead check if the data paths are children of the parent of data_path, i.e. if they are siblings + /* + if (is_sub_dir(boost::filesystem::path(noise_data_path), data_path.parent_path())) { + noise_data_path = make_relative(boost::filesystem::path(noise_data_path), data_path.parent_path()).string(); + } + */ + + if (!uris.empty() && is_sub_dir(boost::filesystem::path(index_convex_segment_paths[0]), data_path.parent_path())) { + for (std::string& segment_path : uris) { + if (segment_path.compare(0, 7, "file://") == 0) { + segment_path = std::string("file://") + make_relative(boost::filesystem::path(segment_path.substr(7, segment_path.size()-7)), data_path.parent_path()).string(); + } + } + } + + std::ofstream out((data_path / boost::filesystem::path("segment_uris.json")).string()); + { + cereal::JSONOutputArchive archive_o(out); + archive_o(*this); + } + } + + template + void serialize(Archive& archive) + { +// archive(cereal::make_nvp("noise_data_path", noise_data_path), +// cereal::make_nvp("uris", uris)); + archive(cereal::make_nvp("uris", uris)); + } + + segment_uris() : uris() //noise_data_path(""), uris() + { + + } +}; + } // namespace dynamic_object_retrieval #endif // SUMMARY_TYPES_H diff --git a/quasimodo/quasimodo_msgs/CMakeLists.txt b/quasimodo/quasimodo_msgs/CMakeLists.txt index fc559d4e..4765607b 100644 --- a/quasimodo/quasimodo_msgs/CMakeLists.txt +++ b/quasimodo/quasimodo_msgs/CMakeLists.txt @@ -51,12 +51,14 @@ add_message_files( retrieval_query.msg retrieval_query_result.msg SOMA2Object.msg + fused_world_state_object.msg ) ## Generate services in the 'srv' folder add_service_files( FILES index_frame.srv + index_cloud.srv model_from_frame.srv cloud_from_model.srv fuse_models.srv @@ -64,6 +66,7 @@ add_service_files( query_cloud.srv simple_query_cloud.srv visualize_query.srv + transform_cloud.srv ) ## Generate actions in the 'action' folder diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp index 31770f81..050807ce 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp @@ -1,4 +1,5 @@ #include "ros/ros.h" +#include #include #include #include @@ -44,15 +45,19 @@ class logging_server { size_t N = res.result.retrieved_clouds.size(); soma_req.objects.resize(N); for (size_t i = 0; i < N; ++i) { + cout << "Retrieved image paths length: " << res.result.retrieved_image_paths.size() << endl; boost::filesystem::path sweep_xml = boost::filesystem::path(res.result.retrieved_image_paths[i].strings[0]).parent_path() / "room.xml"; - auto data = SimpleXMLParser::loadRoomFromXML(sweep_xml.string(), vector(), false, false); + cout << "Sweep xml: " << sweep_xml.string() << endl; + auto data = SimpleXMLParser::loadRoomFromXML(sweep_xml.string(), vector({"RoomIntermediateCloud"}), false, false); boost::posix_time::ptime start_time = data.roomLogStartTime; boost::posix_time::ptime time_t_epoch(boost::gregorian::date(1970,1,1)); boost::posix_time::time_duration diff = start_time - time_t_epoch; Eigen::Affine3d AT; + cout << "Intermediate room cloud transforms length: " << data.vIntermediateRoomCloudTransforms.size() << endl; tf::transformTFToEigen(data.vIntermediateRoomCloudTransforms[0], AT); pcl_ros::transformPointCloud(AT.matrix().cast(), res.result.retrieved_clouds[i], soma_req.objects[i].cloud); //soma_req.objects[i].cloud = res.result.retrieved_clouds[i]; + cout << "Retrieved initial poses length: " << res.result.retrieved_initial_poses.size() << endl; soma_req.objects[i].pose = res.result.retrieved_initial_poses[i].poses[0]; soma_req.objects[i].logtimestamp = diff.total_seconds(); // ros::Time::now().sec; soma_req.objects[i].id = res.result.retrieved_image_paths[i].strings[0]; @@ -75,11 +80,16 @@ class logging_server { ros::ServiceClient soma_service = n.serviceClient("/soma2/insert_objects"); soma_manager::SOMA2InsertObjs::Response soma_resp; - if (soma_service.call(soma_req, soma_resp)) { - cout << "Successfully inserted queries!" << endl; + try { + if (soma_service.call(soma_req, soma_resp)) { + cout << "Successfully inserted queries!" << endl; + } + else { + cout << "Could not connect to SOMA!" << endl; + } } - else { - cout << "Could not connect to SOMA!" << endl; + catch (exception& e) { + cout << "Soma service called failed with: " << e.what() << endl; } /* diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index a6811366..a40241c0 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -91,6 +91,8 @@ class retrieval_node { pn.param("input", retrieval_input, std::string("/models/query")); summary.load(vocabulary_path); + cout << "Loaded summary from: " << (vocabulary_path / "vocabulary_summary.json").string() << endl; + cout << "With data path: " << summary.noise_data_path << endl; // Load and save data in the relative format /* diff --git a/quasimodo/retrieval_processing/src/retrieval_features.cpp b/quasimodo/retrieval_processing/src/retrieval_features.cpp index 6a49aa20..4c55eb47 100644 --- a/quasimodo/retrieval_processing/src/retrieval_features.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_features.cpp @@ -15,6 +15,9 @@ #include #include +#include +#include + #include #include @@ -35,6 +38,7 @@ using SurfelT = SurfelType; using SurfelCloudT = pcl::PointCloud; ros::Publisher pub; +ros::ServiceServer service; /* void test_compute_features(HistCloudT::Ptr& features, CloudT::Ptr& keypoints, CloudT::Ptr& cloud, @@ -136,6 +140,38 @@ void test_compute_features(HistCloudT::Ptr& features, CloudT::Ptr& keypoints, Cl } */ +bool features_service(quasimodo_msgs::transform_cloud::Request& req, quasimodo_msgs::transform_cloud::Response& resp) +{ + double threshold = 0.4; + + SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT); + pcl::fromROSMsg(req.cloud, *surfel_cloud); + HistCloudT::Ptr desc_cloud(new HistCloudT); + CloudT::Ptr kp_cloud(new CloudT); + + NormalCloudT::Ptr normals(new NormalCloudT); + CloudT::Ptr cloud(new CloudT); + cloud->reserve(surfel_cloud->size()); + normals->reserve(surfel_cloud->size()); + for (const SurfelT& s : surfel_cloud->points) { + if (s.confidence < threshold) { + continue; + } + PointT p; + p.getVector3fMap() = s.getVector3fMap(); + p.rgba = s.rgba; + NormalT n; + n.getNormalVector3fMap() = s.getNormalVector3fMap(); + cloud->push_back(p); + normals->push_back(n); + } + dynamic_object_retrieval::compute_features(desc_cloud, kp_cloud, cloud, normals); + pcl::toROSMsg(*desc_cloud, resp.cloud1); + pcl::toROSMsg(*kp_cloud, resp.cloud2); + + return true; +} + void features_callback(const std_msgs::String::ConstPtr& msg) { cout << "Received callback with path " << msg->data << endl; @@ -200,6 +236,7 @@ int main(int argc, char** argv) pn.param("bypass", bypass, 0); pub = n.advertise("/features_done", 1); + service = n.advertiseService("/retrieval_features_service", features_service); ros::Subscriber sub; if (bypass) { diff --git a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp index 7c9c7238..1f32389a 100644 --- a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp @@ -12,6 +12,9 @@ #include #include +#include +#include + #include #include @@ -260,6 +263,49 @@ pair get_offsets_in_data(const boost::filesystem::path& sweep_pa return make_pair(0, 0); } +bool vocabulary_service(quasimodo_msgs::index_cloud::Request& req, quasimodo_msgs::index_cloud::Response& resp) +{ + // let's just create a "ghost" indexing for now, to see if it actually works by comparing to the actual + // segments_summary.json, maybe even have a separate file for the segment uri:s + + // maybe just skip this before we have actually trained the vocabulary, would make things a lot easier, yes, good idea + + if (vt == NULL) { + cout << "Vocabulary not trained yet, can not add any object search observations yet, perform more sweeps first..." << endl; + return false; + } + + HistCloudT::Ptr features(new HistCloudT); + pcl::fromROSMsg(req.cloud, *features); + vector indices; + IndexT index(sweep_offset + sweep_i, offset, 0); // we only have one segment within these observations -> sweep_index = 0 + indices.push_back(index); + AdjacencyT adjacencies; + + if (features->size() > 0) { + vt->append_cloud(features, indices, adjacencies, false); + } + + { + dynamic_object_retrieval::segment_uris uris; + uris.load(vocabulary_path); + uris.uris.push_back(string("mongodb://") + "world_state" + "/" + "quasimodo" + "/" + req.object_id); + uris.save(vocabulary_path); + } + + // This part is new! + { + dynamic_object_retrieval::vocabulary_summary summary; + summary.load(vocabulary_path); + summary.nbr_noise_segments += 1; + summary.nbr_noise_sweeps++; + summary.save(vocabulary_path); + dynamic_object_retrieval::save_vocabulary(*vt, vocabulary_path); + } + + return true; +} + void vocabulary_callback(const std_msgs::String::ConstPtr& msg) { cout << "Received callback with path " << msg->data << endl; @@ -305,11 +351,16 @@ void vocabulary_callback(const std_msgs::String::ConstPtr& msg) dynamic_object_retrieval::sweep_convex_segment_cloud_map clouds(sweep_xml.parent_path()); dynamic_object_retrieval::sweep_convex_feature_cloud_map segment_features(sweep_xml.parent_path()); dynamic_object_retrieval::sweep_convex_keypoint_cloud_map segment_keypoints(sweep_xml.parent_path()); - for (auto tup : dynamic_object_retrieval::zip(segment_features, segment_keypoints, clouds)) { + dynamic_object_retrieval::sweep_convex_segment_map segment_paths(sweep_xml.parent_path()); + // maybe also iterate over the segment paths? + vector segment_strings; + for (auto tup : dynamic_object_retrieval::zip(segment_features, segment_keypoints, clouds, segment_paths)) { HistCloudT::Ptr features_i; CloudT::Ptr keypoints_i; CloudT::Ptr segment; - tie(features_i, keypoints_i, segment) = tup; + boost::filesystem::path segment_path; + tie(features_i, keypoints_i, segment, segment_path) = tup; + segment_strings.push_back(segment_path.string()); features->insert(features->end(), features_i->begin(), features_i->end()); @@ -326,6 +377,16 @@ void vocabulary_callback(const std_msgs::String::ConstPtr& msg) vt->append_cloud(features, indices, adjacencies, false); } + { + dynamic_object_retrieval::segment_uris uris; + uris.load(vocabulary_path); + uris.uris.reserve(uris.uris.size() + segment_strings.size()); + for (const string& segment_path : segment_strings) { + uris.push_back(string("file://") + segment_path); + } + uris.save(vocabulary_path); + } + // This part is new! dynamic_object_retrieval::vocabulary_summary summary; summary.load(vocabulary_path); From 88c4a7676b4cd7cb67f020a35b7582a26af0efe0 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Thu, 16 Jun 2016 15:01:48 +0200 Subject: [PATCH 003/255] Fixed compilation errors, mask and retrieval changes remain --- .../dynamic_object_retrieval/summary_types.h | 8 +++++--- .../src/retrieval_vocabulary.cpp | 13 +++++++++++-- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h index dcd507e7..47ab651d 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h @@ -242,7 +242,7 @@ struct segment_uris { void load(const boost::filesystem::path& data_path) { - std::ifstream in((data_path / boost::filesystem::path(vocabulary) / boost::filesystem::path("segment_uris.json")).string()); + std::ifstream in((data_path / boost::filesystem::path("segment_uris.json")).string()); { cereal::JSONInputArchive archive_i(in); archive_i(*this); @@ -253,7 +253,8 @@ struct segment_uris { } */ - if (!uris.empty() && !boost::filesystem::path(index_convex_segment_paths[0]).is_absolute()) { + // we can actually assume that 0 is a file since the first ones are used to train the vocabulary + if (!uris.empty() && !boost::filesystem::path(uris[0].substr(7, uris[0].size()-7)).is_absolute()) { for (std::string& segment_path : uris) { //segment_path = boost::filesystem::canonical(segment_path, data_path).string(); //HMMM, this will start with a slash? @@ -275,7 +276,8 @@ struct segment_uris { } */ - if (!uris.empty() && is_sub_dir(boost::filesystem::path(index_convex_segment_paths[0]), data_path.parent_path())) { + // we can actually assume that 0 is a file since the first ones are used to train the vocabulary + if (!uris.empty() && is_sub_dir(boost::filesystem::path(uris[0].substr(7, uris[0].size()-7)), data_path.parent_path())) { for (std::string& segment_path : uris) { if (segment_path.compare(0, 7, "file://") == 0) { segment_path = std::string("file://") + make_relative(boost::filesystem::path(segment_path.substr(7, segment_path.size()-7)), data_path.parent_path()).string(); diff --git a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp index 1f32389a..662efbd5 100644 --- a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp @@ -127,6 +127,8 @@ void train_vocabulary(const boost::filesystem::path& data_path) AdjacencyT adjacencies; vector indices; + dynamic_object_retrieval::segment_uris uris; + size_t counter = 0; // index among all segments size_t sweep_i; // index of sweep size_t last_sweep = 0; // last index of sweep @@ -193,6 +195,9 @@ void train_vocabulary(const boost::filesystem::path& data_path) indices.push_back(index); } + // here we insert in the uris + uris.uris.push_back(string("file://") + segment_path.string()); + ++counter; ++sweep_counter; if (sweep_i >= min_training_sweeps && islast) { @@ -214,6 +219,7 @@ void train_vocabulary(const boost::filesystem::path& data_path) summary.nbr_annotated_sweeps = 0; summary.save(vocabulary_path); + uris.save(vocabulary_path); dynamic_object_retrieval::save_vocabulary(*vt, vocabulary_path); } @@ -278,7 +284,10 @@ bool vocabulary_service(quasimodo_msgs::index_cloud::Request& req, quasimodo_msg HistCloudT::Ptr features(new HistCloudT); pcl::fromROSMsg(req.cloud, *features); vector indices; - IndexT index(sweep_offset + sweep_i, offset, 0); // we only have one segment within these observations -> sweep_index = 0 + // the question is what offsets we should assign these, I think in reality it doesn't matter, possibly the sweep offset might not get set automatically + // let's just try 0, 0 for now + //IndexT index(sweep_offset + sweep_i, offset, 0); // we only have one segment within these observations -> sweep_index = 0 + IndexT index(0, 0, 0); // we only have one segment within these observations -> sweep_index = 0 indices.push_back(index); AdjacencyT adjacencies; @@ -382,7 +391,7 @@ void vocabulary_callback(const std_msgs::String::ConstPtr& msg) uris.load(vocabulary_path); uris.uris.reserve(uris.uris.size() + segment_strings.size()); for (const string& segment_path : segment_strings) { - uris.push_back(string("file://") + segment_path); + uris.uris.push_back(string("file://") + segment_path); } uris.save(vocabulary_path); } From 9f5818632b374ff2b2ca335d829dbe68019bbafb Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 20 Jun 2016 12:31:07 +0200 Subject: [PATCH 004/255] Added some support for querying mongo also, to be tested --- .../dynamic_retrieval.h | 91 +++++++++++++++++++ 1 file changed, 91 insertions(+) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index 6ddfe10d..c7e81184 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -17,8 +17,15 @@ #include #include #include +#include #define WITH_SURFEL_NORMALS 1 +#define WITH_UNIVERSAL_URIS 1 + +#if WITH_UNIVERSAL_URIS +#include +#include +#endif using PointT = pcl::PointXYZRGB; using CloudT = pcl::PointCloud; @@ -42,6 +49,87 @@ struct path_result > { template std::vector get_retrieved_paths(const std::vector& scores, const vocabulary_summary& summary) { + // OK, this is where we add the new type of segment_uris +#if WITH_UNIVERSAL_URIS + + ros::NodeHandle n("/dynamic_retrieval"); + size_t offset = summary.nbr_noise_segments; + segment_uris uris; + uris.load(boost::filesystem::path(summary.noise_data_path)); + mongodb_store::MessageStoreProxy* message_store = NULL; + for (IndexT s : scores) { + if (s.index >= offset) { + std::cout << "URI loading does not support noise+annotated data structure!" << std::endl; + exit(-1); + } + std::string uri = uris.uris[s.index]; + if (uri.compare(0, 10, "mongodb://") == 0) { + std::string db_uri = uri.substr(7, uri.size()-7); + std::vector strs; + boost::split(strs, db_uri, boost::is_any_of("/")); + std::cout << "Initializing message story proxy with database: " << strs[0] << ", collection: " << strs[1] << std::endl; + message_store = new mongodb_store::MessageStoreProxy(n, strs[1], strs[0]); + break; + } + } + + std::vector retrieved_paths; + for (IndexT s : scores) { + // should we write to some temporary files and return the paths to those? + // e.g. in ~/.ros/quasimodo + // the question is also if we should write a dummy surfel_map.pcd containing the cloud and a separate object.pcd with only points + // another possibility would be to actually just write surfel_map.pcd, that should be enough? + // the big problem is that we won't able to get any of the info in room.xml + if (s.index >= offset) { + std::cout << "URI loading does not support noise+annotated data structure!" << std::endl; + exit(-1); + } + std::string uri = uris.uris[s.index]; + if (uri.compare(0, 7, "file://") == 0) { + retrieved_paths.push_back(boost::filesystem::path(uri.substr(7, uri.size()-7))); + } + else if (uri.compare(0, 10, "mongodb://") == 0) { + // don't delete anything here, instead have a cache based on the database id:s + // use boost split to split separate the uri into database / collection / id + std::string db_uri = uri.substr(7, uri.size()-7); + std::vector strs; + boost::split(strs, db_uri, boost::is_any_of("/")); + + // how do we get the home directory correctly? + boost::filesystem::path temp_path = boost::filesystem::path("~/.ros/quasimodo/temp") / (strs[2] + ".pcd"); + + if (boost::filesystem::exists(temp_path)) { + retrieved_paths.push_back(temp_path); + continue; + } + + if (!boost::filesystem::exists("~/.ros/quasimodo/temp")) { + boost::filesystem::create_directories("~/.ros/quasimodo/temp"); + } + + boost::shared_ptr message; + mongo::BSONObj bson_message; + std::tie(message, bson_message) = message_store->queryID(strs[2]); + SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT); + pcl::fromROSMsg(message->surfel_cloud, *surfel_cloud); + pcl::io::savePCDFileBinary(temp_path.string(), *surfel_cloud); + retrieved_paths.push_back(temp_path); + + // [0] - database, [1] - collection, [2] - mongodb object id + + } + else { + std::cout << "Loading from segment URI problem: URI not well formed!" << std::endl; + exit(-1); + } + } + + if (message_store != NULL) { // redundant but clearer + delete message_store; + } + + return retrieved_paths; +#else // here we'll make use of the info saved in the data_summaries // maybe we should cache these as well as they might get big? data_summary noise_summary; @@ -49,6 +137,8 @@ std::vector get_retrieved_paths(const std::vector retrieved_paths; size_t offset = summary.nbr_noise_segments; for (IndexT s : scores) { @@ -62,6 +152,7 @@ std::vector get_retrieved_paths(const std::vector get_retrieved_paths(const std::vector& scores, const vocabulary_summary& summary); From 6ff72ba504363f5a32f9794ccae1e075a840c432 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 21 Jun 2016 12:50:00 +0200 Subject: [PATCH 005/255] More stuff to integrate object search --- quasimodo/quasimodo_msgs/CMakeLists.txt | 1 + .../msg/fused_world_state_object.msg | 2 + quasimodo/quasimodo_msgs/srv/index_cloud.srv | 4 + .../quasimodo_msgs/srv/mask_pointclouds.srv | 4 + .../quasimodo_msgs/srv/transform_cloud.srv | 4 + .../create_object_search_observation.py | 157 ++++++++++++++++++ .../src/retrieval_segmentation.cpp | 37 +++++ 7 files changed, 209 insertions(+) create mode 100644 quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg create mode 100644 quasimodo/quasimodo_msgs/srv/index_cloud.srv create mode 100644 quasimodo/quasimodo_msgs/srv/mask_pointclouds.srv create mode 100644 quasimodo/quasimodo_msgs/srv/transform_cloud.srv create mode 100755 quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py diff --git a/quasimodo/quasimodo_msgs/CMakeLists.txt b/quasimodo/quasimodo_msgs/CMakeLists.txt index 4765607b..07462af5 100644 --- a/quasimodo/quasimodo_msgs/CMakeLists.txt +++ b/quasimodo/quasimodo_msgs/CMakeLists.txt @@ -67,6 +67,7 @@ add_service_files( simple_query_cloud.srv visualize_query.srv transform_cloud.srv + mask_pointclouds.srv ) ## Generate actions in the 'action' folder diff --git a/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg new file mode 100644 index 00000000..94f230b1 --- /dev/null +++ b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg @@ -0,0 +1,2 @@ +sensor_msgs/PointCloud2 surfel_cloud +string object_id diff --git a/quasimodo/quasimodo_msgs/srv/index_cloud.srv b/quasimodo/quasimodo_msgs/srv/index_cloud.srv new file mode 100644 index 00000000..04b4e7cb --- /dev/null +++ b/quasimodo/quasimodo_msgs/srv/index_cloud.srv @@ -0,0 +1,4 @@ +sensor_msgs/PointCloud2 cloud +string object_id +--- +uint64 id diff --git a/quasimodo/quasimodo_msgs/srv/mask_pointclouds.srv b/quasimodo/quasimodo_msgs/srv/mask_pointclouds.srv new file mode 100644 index 00000000..33fb3c54 --- /dev/null +++ b/quasimodo/quasimodo_msgs/srv/mask_pointclouds.srv @@ -0,0 +1,4 @@ +sensor_msgs/PointCloud2[] clouds +sensor_msgs/Image[] masks +--- +sensor_msgs/PointCloud2[] clouds diff --git a/quasimodo/quasimodo_msgs/srv/transform_cloud.srv b/quasimodo/quasimodo_msgs/srv/transform_cloud.srv new file mode 100644 index 00000000..8d505095 --- /dev/null +++ b/quasimodo/quasimodo_msgs/srv/transform_cloud.srv @@ -0,0 +1,4 @@ +sensor_msgs/PointCloud2 cloud +--- +sensor_msgs/PointCloud2 cloud1 +sensor_msgs/PointCloud2 cloud2 diff --git a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py new file mode 100755 index 00000000..6fb79090 --- /dev/null +++ b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py @@ -0,0 +1,157 @@ +#!/usr/bin/env python + +import roslib +import rospy +from sensor_msgs.msg import PointCloud2, PointField +#from world_modeling.srv import * +from cv_bridge import CvBridge, CvBridgeError +import cv2 + +# SOMA2 stuff +from soma2_msgs.msg import SOMA2Object +from mongodb_store.message_store import MessageStoreProxy +from soma_manager.srv import * +from geometry_msgs.msg import Pose, Transform +from observation_registration_services.srv import ProcessRegisteredViews, ProcessRegisteredViewsRequest, ProcessRegisteredViewsResponse +from soma_io.state import World, Object +from datetime import datetime, timedelta +from quasimodo_msgs.msg import fused_world_state_object +from quasimodo_msgs.srv import transform_cloud, transform_cloudRequest, transform_cloudResponse +from quasimodo_msgs.srv import index_cloud, index_cloudRequest, index_cloudResponse +from quasimodo_msgs.srv import mask_pointclouds, mask_pointcloudsRequest, mask_pointcloudsResponse +import time + +UPDATE_INT_MINUTES = 100000.0 + +def chron_callback(): + + print("making query") + soma_query = rospy.ServiceProxy('soma2/query_db',SOMA2QueryObjs) + print("done query") + + # Query all observations during the last 30 mins + query = SOMA2QueryObjsRequest() + query.query_type = 0 + query.objecttypes=['unknown'] + query.uselowertime = True + query.usedates = True + dt_obj = datetime.now() - timedelta(minutes=UPDATE_INT_MINUTES) #fromtimestamp(query.lowerdate + query.lowerhour = dt_obj.hour + print "Hour %d" % query.lowerhour + query.lowerminutes = dt_obj.minute + print "Minute %d" % query.lowerminutes + + response = soma_query(query) + + if not response.objects: + print("No SOMA objects!") + else: + print("got " + str(len(response.objects)) + " SOMA objects") + msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') + world_model = World(server_host='localhost',server_port=62345) + #print world_model + for x in response.objects: + print(x.id) + wo = world_model.get_object(x.id) + + transforms = [] #wo._poses + req = mask_pointcloudsRequest() + for obs, pose in zip(wo._observations, wo._poses): + + #rgb = fo.get_message("/head_xtion/rgb/image_rect_color") + req.clouds.append(obs.get_message("/head_xtion/depth_registered/points")) + req.masks.append(obs.get_message("rgb_mask")) + + #pointclouds.append(obs.get_message("object_cloud_camframe")) + #could we set everything that's in the background to inf/0? + transform = Transform() + transform.rotation.x = pose.quaternion.x + transform.rotation.y = pose.quaternion.y + transform.rotation.z = pose.quaternion.z + transform.rotation.w = pose.quaternion.w + transform.translation.x = pose.position.x + transform.translation.y = pose.position.y + transform.translation.z = pose.position.z + + transforms.append(transform) + + rospy.wait_for_service('retrieval_segmentation_server') + try: + surfelize_server = rospy.ServiceProxy('retrieval_segmentation_server', mask_pointclouds) + resp = surfelize_server(request) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + return + + #sensor_msgs/PointCloud2[] registered_views + #geometry_msgs/Transform[] registered_views_transforms + #std_msgs/Float32[] intrinsics # can be empty + #--- + #sensor_msgs/PointCloud2 processed_cloud + request = ProcessRegisteredViewsRequest() + request.registered_views = resp.clouds + # This should possibly be transformed to the frame of the first + # observation to make visualization a bit easier + request.registered_views_transforms = transforms + + rospy.wait_for_service('surfelize_server') + try: + surfelize_server = rospy.ServiceProxy('surfelize_server', ProcessRegisteredViews) + resp = surfelize_server(request) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + return + + new_obj = fused_world_state_object() + new_obj.surfel_cloud = resp.processed_cloud + new_obj.object_id = x.id + + rospy.wait_for_service('retrieval_features_service') + req = transform_cloudRequest() + req.cloud = new_obj.surfel_cloud + try: + surfelize_server = rospy.ServiceProxy('retrieval_features_service', transform_cloud) + resp = surfelize_server(request) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + return + + new_obj.features = resp.cloud1 + new_obj.keypoints = resp.cloud2 + + object_id = msg_store.insert(new_obj) + + rospy.wait_for_service('retrieval_vocabulary_service') + req = index_cloudRequest() + req.cloud = new_obj.surfel_cloud + req.object_id = object_id + try: + surfelize_server = rospy.ServiceProxy('retrieval_vocabulary_service', index_cloud) + resp = surfelize_server(request) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + return + + new_obj.vocabulary_index = resp.id + msg_store.update_id(object_id, new_obj) + # Now, simply save this in mongodb, or maybe extract features first? + + print object_id + + break + + + + print("done") + + +if __name__ == '__main__': + rospy.init_node('create_object_search_observation', anonymous = False) + print("getting soma service") + rospy.wait_for_service('soma2/query_db') + print("done initializing") + + r = rospy.Rate(1.0/(60.0*UPDATE_INT_MINUTES)) # 10hz + while not rospy.is_shutdown(): + chron_callback() + r.sleep() diff --git a/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp b/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp index 02accb90..1c4e1338 100644 --- a/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp @@ -13,6 +13,9 @@ #include #include +#include + +#include #define VISUALIZE 1 @@ -61,6 +64,40 @@ double threshold; dynamic_object_retrieval::data_summary data_summary; boost::filesystem::path data_path; +bool segmentation_service(quasimodo_msgs::mask_pointclouds::Request& req, quasimodo_msgs::mask_pointclouds::Response& resp) +{ + for (size_t i = 0; i < req.clouds.size(); ++i) { + CloudT::Ptr cloud(new CloudT); + pcl::fromROSMsg(req.clouds[i], *cloud); + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(req.masks[i], sensor_msgs::image_encodings::BGR8); + } + catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + exit(-1); + } + cv::Mat mask; + cv::cvtColor(cv_ptr->image, mask, CV_8UC1); + + CloudT::Ptr masked_cloud(new CloudT); + for (size_t y = 0; y < mask.rows; ++y) { + for (size_t x = 0; x < mask.cols; ++x) { + size_t index = y*mask.cols + x; + if (mask.at(y, x) = 255) { + masked_cloud->push_back(cloud->at(index)); + } + } + } + + sensor_msgs::PointCloud2 masked_msg; + pcl::toROSMsg(*masked_cloud, masked_msg); + resp.clouds.push_back(masked_msg); + } + + return true; +} + void segmentation_callback(const std_msgs::String::ConstPtr& msg) { data_summary.load(data_path); From d239d96f2d703ffb8df55250876f270a120b9aaf Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 21 Jun 2016 17:03:33 +0200 Subject: [PATCH 006/255] Almost finished integration --- .../msg/fused_world_state_object.msg | 3 + .../quasimodo_object_search/CMakeLists.txt | 181 ++++++++++++++++++ quasimodo/quasimodo_object_search/package.xml | 50 +++++ .../create_object_search_observation.py | 29 +-- .../src/retrieval_features.cpp | 6 +- .../src/retrieval_segmentation.cpp | 13 +- .../src/retrieval_vocabulary.cpp | 17 +- 7 files changed, 280 insertions(+), 19 deletions(-) create mode 100644 quasimodo/quasimodo_object_search/CMakeLists.txt create mode 100644 quasimodo/quasimodo_object_search/package.xml diff --git a/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg index 94f230b1..bfed9aa7 100644 --- a/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg +++ b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg @@ -1,2 +1,5 @@ sensor_msgs/PointCloud2 surfel_cloud +sensor_msgs/PointCloud2 features +sensor_msgs/PointCloud2 keypoints string object_id +uint64 vocabulary_id diff --git a/quasimodo/quasimodo_object_search/CMakeLists.txt b/quasimodo/quasimodo_object_search/CMakeLists.txt new file mode 100644 index 00000000..1a70e6bb --- /dev/null +++ b/quasimodo/quasimodo_object_search/CMakeLists.txt @@ -0,0 +1,181 @@ +cmake_minimum_required(VERSION 2.8.3) +project(quasimodo_object_search) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES quasimodo_object_search2 +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) + +## Declare a C++ library +# add_library(quasimodo_object_search2 +# src/${PROJECT_NAME}/quasimodo_object_search2.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(quasimodo_object_search2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(quasimodo_object_search2_node src/quasimodo_object_search2_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(quasimodo_object_search2_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(quasimodo_object_search2_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS quasimodo_object_search2 quasimodo_object_search2_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_quasimodo_object_search2.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/quasimodo/quasimodo_object_search/package.xml b/quasimodo/quasimodo_object_search/package.xml new file mode 100644 index 00000000..2ac15879 --- /dev/null +++ b/quasimodo/quasimodo_object_search/package.xml @@ -0,0 +1,50 @@ + + + quasimodo_object_search + 0.0.0 + The quasimodo_object_search2 package + + + + + nbore + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + + diff --git a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py index 6fb79090..ae1fac31 100755 --- a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py +++ b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py @@ -75,10 +75,11 @@ def chron_callback(): transforms.append(transform) - rospy.wait_for_service('retrieval_segmentation_server') + print "Waiting for retrieval_segmentation_service..." + rospy.wait_for_service('retrieval_segmentation_service') try: - surfelize_server = rospy.ServiceProxy('retrieval_segmentation_server', mask_pointclouds) - resp = surfelize_server(request) + surfelize_server = rospy.ServiceProxy('retrieval_segmentation_service', mask_pointclouds) + resp = surfelize_server(req) except rospy.ServiceException, e: print "Service call failed: %s"%e return @@ -88,16 +89,17 @@ def chron_callback(): #std_msgs/Float32[] intrinsics # can be empty #--- #sensor_msgs/PointCloud2 processed_cloud - request = ProcessRegisteredViewsRequest() - request.registered_views = resp.clouds + req = ProcessRegisteredViewsRequest() + req.registered_views = resp.clouds # This should possibly be transformed to the frame of the first # observation to make visualization a bit easier - request.registered_views_transforms = transforms + req.registered_views_transforms = transforms + print "Waiting for surfelize_server..." rospy.wait_for_service('surfelize_server') try: surfelize_server = rospy.ServiceProxy('surfelize_server', ProcessRegisteredViews) - resp = surfelize_server(request) + resp = surfelize_server(req) except rospy.ServiceException, e: print "Service call failed: %s"%e return @@ -106,12 +108,13 @@ def chron_callback(): new_obj.surfel_cloud = resp.processed_cloud new_obj.object_id = x.id + print "Waiting for retrieval_features_service..." rospy.wait_for_service('retrieval_features_service') req = transform_cloudRequest() - req.cloud = new_obj.surfel_cloud + req.cloud = resp.processed_cloud try: surfelize_server = rospy.ServiceProxy('retrieval_features_service', transform_cloud) - resp = surfelize_server(request) + resp = surfelize_server(req) except rospy.ServiceException, e: print "Service call failed: %s"%e return @@ -121,18 +124,19 @@ def chron_callback(): object_id = msg_store.insert(new_obj) + print "Waiting for retrieval_vocabulary_service..." rospy.wait_for_service('retrieval_vocabulary_service') req = index_cloudRequest() - req.cloud = new_obj.surfel_cloud + req.cloud = new_obj.features req.object_id = object_id try: surfelize_server = rospy.ServiceProxy('retrieval_vocabulary_service', index_cloud) - resp = surfelize_server(request) + resp = surfelize_server(req) except rospy.ServiceException, e: print "Service call failed: %s"%e return - new_obj.vocabulary_index = resp.id + new_obj.vocabulary_id = resp.id msg_store.update_id(object_id, new_obj) # Now, simply save this in mongodb, or maybe extract features first? @@ -154,4 +158,5 @@ def chron_callback(): r = rospy.Rate(1.0/(60.0*UPDATE_INT_MINUTES)) # 10hz while not rospy.is_shutdown(): chron_callback() + break r.sleep() diff --git a/quasimodo/retrieval_processing/src/retrieval_features.cpp b/quasimodo/retrieval_processing/src/retrieval_features.cpp index 4c55eb47..7fa7f128 100644 --- a/quasimodo/retrieval_processing/src/retrieval_features.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_features.cpp @@ -141,11 +141,13 @@ void test_compute_features(HistCloudT::Ptr& features, CloudT::Ptr& keypoints, Cl */ bool features_service(quasimodo_msgs::transform_cloud::Request& req, quasimodo_msgs::transform_cloud::Response& resp) -{ +{ double threshold = 0.4; SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT); pcl::fromROSMsg(req.cloud, *surfel_cloud); + cout << "Got service cloud with size: " << surfel_cloud->size() << endl; + HistCloudT::Ptr desc_cloud(new HistCloudT); CloudT::Ptr kp_cloud(new CloudT); @@ -165,6 +167,8 @@ bool features_service(quasimodo_msgs::transform_cloud::Request& req, quasimodo_m cloud->push_back(p); normals->push_back(n); } + cout << "Service points after confidence " << threshold << ": " << cloud->size() << endl; + dynamic_object_retrieval::compute_features(desc_cloud, kp_cloud, cloud, normals); pcl::toROSMsg(*desc_cloud, resp.cloud1); pcl::toROSMsg(*kp_cloud, resp.cloud2); diff --git a/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp b/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp index 1c4e1338..953ddf8e 100644 --- a/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_segmentation.cpp @@ -60,6 +60,7 @@ int colormap[][3] = { ros::Publisher pub; ros::Publisher vis_cloud_pub; +ros::ServiceServer service; double threshold; dynamic_object_retrieval::data_summary data_summary; boost::filesystem::path data_path; @@ -78,20 +79,21 @@ bool segmentation_service(quasimodo_msgs::mask_pointclouds::Request& req, quasim exit(-1); } cv::Mat mask; - cv::cvtColor(cv_ptr->image, mask, CV_8UC1); + cv::cvtColor(cv_ptr->image, mask, CV_BGR2GRAY); - CloudT::Ptr masked_cloud(new CloudT); for (size_t y = 0; y < mask.rows; ++y) { for (size_t x = 0; x < mask.cols; ++x) { size_t index = y*mask.cols + x; - if (mask.at(y, x) = 255) { - masked_cloud->push_back(cloud->at(index)); + if (int(mask.at(y, x)) != 255) { + cloud->points[index].x = std::numeric_limits::infinity(); + cloud->points[index].y = std::numeric_limits::infinity(); + cloud->points[index].z = std::numeric_limits::infinity(); } } } sensor_msgs::PointCloud2 masked_msg; - pcl::toROSMsg(*masked_cloud, masked_msg); + pcl::toROSMsg(*cloud, masked_msg); resp.clouds.push_back(masked_msg); } @@ -229,6 +231,7 @@ int main(int argc, char** argv) pub = n.advertise("/segmentation_done", 1); vis_cloud_pub = n.advertise("/retrieval_processing/segmentation_cloud", 1); + service = n.advertiseService("/retrieval_segmentation_service", segmentation_service); ros::Subscriber sub; if (bypass) { diff --git a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp index 662efbd5..08fd75c7 100644 --- a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp @@ -54,6 +54,7 @@ using AdjacencyT = vector > >; // does that mean we need to provide the querying // interface as well? maybe ros::Publisher pub; +ros::ServiceServer service; int min_training_sweeps; size_t nbr_added_sweeps; boost::filesystem::path vocabulary_path; @@ -297,7 +298,18 @@ bool vocabulary_service(quasimodo_msgs::index_cloud::Request& req, quasimodo_msg { dynamic_object_retrieval::segment_uris uris; - uris.load(vocabulary_path); + if (boost::filesystem::exists(vocabulary_path / "segment_uris.json")) + { + uris.load(vocabulary_path); + } + else { + // load all the segments summaries and push them as uris + dynamic_object_retrieval::data_summary segments_summary; + segments_summary.load(vocabulary_path.parent_path()); + for (const string& segment_path : segments_summary.index_convex_segment_paths) { + uris.uris.push_back(string("file://") + segment_path); + } + } uris.uris.push_back(string("mongodb://") + "world_state" + "/" + "quasimodo" + "/" + req.object_id); uris.save(vocabulary_path); } @@ -306,6 +318,7 @@ bool vocabulary_service(quasimodo_msgs::index_cloud::Request& req, quasimodo_msg { dynamic_object_retrieval::vocabulary_summary summary; summary.load(vocabulary_path); + resp.id = summary.nbr_noise_segments; summary.nbr_noise_segments += 1; summary.nbr_noise_sweeps++; summary.save(vocabulary_path); @@ -444,12 +457,14 @@ int main(int argc, char** argv) vt = new VocT(vocabulary_path.string()); vt->set_min_match_depth(3); dynamic_object_retrieval::load_vocabulary(*vt, vocabulary_path); + vt->set_cache_path(vocabulary_path.string()); } else { vt = NULL; } pub = n.advertise("/vocabulary_done", 1); + service = n.advertiseService("/retrieval_vocabulary_service", vocabulary_service); ros::Subscriber sub; if (bypass) { From 85b98f494650cd46f9106b929c7102812a87e358 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Wed, 29 Jun 2016 23:23:12 +0200 Subject: [PATCH 007/255] Added initial code for querying mongodb --- .../msg/fused_world_state_object.msg | 5 + .../create_object_search_observation.py | 11 ++ .../scripts/retrieve_object_search.py | 102 ++++++++++++++++++ 3 files changed, 118 insertions(+) create mode 100644 quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py diff --git a/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg index bfed9aa7..2733819c 100644 --- a/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg +++ b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg @@ -3,3 +3,8 @@ sensor_msgs/PointCloud2 features sensor_msgs/PointCloud2 keypoints string object_id uint64 vocabulary_id +sensor_msgs/Image[] images +sensor_msgs/Image[] depths +sensor_msgs/Image[] masks +geometry_msgs/Pose[] transforms + diff --git a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py index ae1fac31..b246153b 100755 --- a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py +++ b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py @@ -19,6 +19,7 @@ from quasimodo_msgs.srv import transform_cloud, transform_cloudRequest, transform_cloudResponse from quasimodo_msgs.srv import index_cloud, index_cloudRequest, index_cloudResponse from quasimodo_msgs.srv import mask_pointclouds, mask_pointcloudsRequest, mask_pointcloudsResponse +from geometry_msgs.msg import Pose import time UPDATE_INT_MINUTES = 100000.0 @@ -108,6 +109,16 @@ def chron_callback(): new_obj.surfel_cloud = resp.processed_cloud new_obj.object_id = x.id + for obs, pose in zip(wo._observations, wo._poses): + new_obj.transforms.append(Pose()) + new_obj.transforms[-1].position.x = pose.position.x + new_obj.transforms[-1].position.y = pose.position.y + new_obj.transforms[-1].position.z = pose.position.z + new_obj.transforms[-1].orientation.x = pose.quaternion.x + new_obj.transforms[-1].orientation.y = pose.quaternion.y + new_obj.transforms[-1].orientation.z = pose.quaternion.z + new_obj.transforms[-1].orientation.w = pose.quaternion.w + print "Waiting for retrieval_features_service..." rospy.wait_for_service('retrieval_features_service') req = transform_cloudRequest() diff --git a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py new file mode 100644 index 00000000..4291af63 --- /dev/null +++ b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python + +import roslib +import rospy +from sensor_msgs.msg import PointCloud2, PointField +#from world_modeling.srv import * +from cv_bridge import CvBridge, CvBridgeError +import cv2 + +# SOMA2 stuff +from soma2_msgs.msg import SOMA2Object +from mongodb_store.message_store import MessageStoreProxy +from soma_manager.srv import * +from geometry_msgs.msg import Pose, Transform +from observation_registration_services.srv import ProcessRegisteredViews, ProcessRegisteredViewsRequest, ProcessRegisteredViewsResponse +from soma_io.state import World, Object +from datetime import datetime, timedelta +from quasimodo_msgs.msg import fused_world_state_object +from quasimodo_msgs.srv import transform_cloud, transform_cloudRequest, transform_cloudResponse +from quasimodo_msgs.srv import index_cloud, index_cloudRequest, index_cloudResponse +from quasimodo_msgs.srv import mask_pointclouds, mask_pointcloudsRequest, mask_pointcloudsResponse +from quasimodo_msgs.msg import retrieval_query +from geometry_msgs.msg import Pose +import time + +UPDATE_INT_MINUTES = 100000.0 + +pub = () + +def retrieval_callback(object_id): + + world_model = World(server_host='localhost',server_port=62345) + print(object_id) + wo = world_model.get_object(object_id) + + transforms = [] #wo._poses + images = [] + depths = [] + masks = [] + cameras = [] + req = mask_pointcloudsRequest() + for obs, pose in zip(wo._observations, wo._poses): + + images.append(fo.get_message("/head_xtion/rgb/image_rect_color")) + depthmat = np.zeros((480,640,1), np.uint16) + # have to be careful here, Johan has a different encoding + depths.append(CvBridge().cv2_to_imgmsg(depthmat, "mono16")) + masks.append(fo.get_message("rgb_mask")) + cameras = fo.get_message("/head_xtion/depth_registered/sw_registered/camera_info") + req.clouds.append(obs.get_message("/head_xtion/depth_registered/points")) + req.masks.append(obs.get_message("rgb_mask")) + + transform = Transform() + transform.rotation.x = pose.quaternion.x + transform.rotation.y = pose.quaternion.y + transform.rotation.z = pose.quaternion.z + transform.rotation.w = pose.quaternion.w + transform.translation.x = pose.position.x + transform.translation.y = pose.position.y + transform.translation.z = pose.position.z + + transforms.append(transform) + + print "Waiting for retrieval_segmentation_service..." + rospy.wait_for_service('retrieval_segmentation_service') + try: + surfelize_server = rospy.ServiceProxy('retrieval_segmentation_service', mask_pointclouds) + resp = surfelize_server(req) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + return + + req = ProcessRegisteredViewsRequest() + req.registered_views = resp.clouds + req.registered_views_transforms = transforms + + print "Waiting for surfelize_server..." + rospy.wait_for_service('surfelize_server') + try: + surfelize_server = rospy.ServiceProxy('surfelize_server', ProcessRegisteredViews) + resp = surfelize_server(req) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + return + + query = retrieval_query() + query.camera = cameras[0] + query.cloud = resp.processed_cloud + query.depth = depths[0] + query.image = images[0] + query.mask = masks[0] + query.number_query = 10 + query.room_transform = transforms[0] + + pub.publish(query) + + print("done") + +if __name__ == '__main__': + rospy.init_node('retrieve_object_search', anonymous = False) + pub = rospy.Publisher("/models/query", retrieval_query) + rospy.spin() From 51ac7bda0909532f9d95bb02784ba861e6f39a63 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Wed, 29 Jun 2016 23:38:02 +0200 Subject: [PATCH 008/255] Added a subscriber to the mongodb search function --- .../scripts/create_object_search_observation.py | 1 + .../quasimodo_object_search/scripts/retrieve_object_search.py | 2 ++ 2 files changed, 3 insertions(+) diff --git a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py index b246153b..3ef66e97 100755 --- a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py +++ b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py @@ -49,6 +49,7 @@ def chron_callback(): else: print("got " + str(len(response.objects)) + " SOMA objects") msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') + # This hardcoding is no good! world_model = World(server_host='localhost',server_port=62345) #print world_model for x in response.objects: diff --git a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py index 4291af63..3442806f 100644 --- a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py +++ b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py @@ -29,6 +29,7 @@ def retrieval_callback(object_id): + # This hardcoding is no good! world_model = World(server_host='localhost',server_port=62345) print(object_id) wo = world_model.get_object(object_id) @@ -99,4 +100,5 @@ def retrieval_callback(object_id): if __name__ == '__main__': rospy.init_node('retrieve_object_search', anonymous = False) pub = rospy.Publisher("/models/query", retrieval_query) + sub = rospy.Subscriber("/models/mongodb_query", std_msgs.String, callback=retrieval_callback) rospy.spin() From b8516217036e601b95ed1164725228dc1748bfe7 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Wed, 29 Jun 2016 23:53:11 +0200 Subject: [PATCH 009/255] Compiling --- .../src/quasimodo_retrieval_node.cpp | 129 ++++++++++++++---- 1 file changed, 105 insertions(+), 24 deletions(-) diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index a40241c0..28db9460 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -28,6 +28,13 @@ #include #include +#define WITH_UNIVERSAL_URIS 1 + +#if WITH_UNIVERSAL_URIS +#include +#include +#endif + using namespace std; using PointT = pcl::PointXYZRGB; @@ -200,6 +207,62 @@ class retrieval_node { return images; } + tuple, vector, vector, vector > + generate_images_for_mongodb_object(const CloudT::Ptr& cloud, const Eigen::Matrix3f& K, + const boost::filesystem::path& sweep_xml, + const vector >& transforms) + { + tuple, vector, vector, vector > images; + + //int height = 480; + //int width = 640; + + string mongodb_id = sweep_xml.parent_path().stem().string(); + //string mongodb_uri = string("/world_state/quasimodo/") / mongodb_id; + + mongodb_store::MessageStoreProxy message_store_quasimodo(n, "quasimodo", "world_state"); + pair, mongo::BSONObj> query = message_store_quasimodo.queryID(mongodb_id); + + for (int i = 0; i < query.first->images.size(); ++i) { + cv_bridge::CvImagePtr cv_mask_ptr; + try { + cv_mask_ptr = cv_bridge::toCvCopy(query.first->masks[i], sensor_msgs::image_encodings::BGR8); + } + catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + exit(-1); + } + cv::Mat mask; + cv::cvtColor(cv_mask_ptr->image, mask, CV_BGR2GRAY); + + cv_bridge::CvImagePtr cv_image_ptr; + try { + cv_image_ptr = cv_bridge::toCvCopy(query.first->images[i], sensor_msgs::image_encodings::BGR8); + } + catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + exit(-1); + } + + cv_bridge::CvImagePtr cv_depth_ptr; + try { + cv_depth_ptr = cv_bridge::toCvCopy(query.first->depths[i], sensor_msgs::image_encodings::MONO16); + } + catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + exit(-1); + } + + get<0>(images).push_back(mask); + get<1>(images).push_back(cv_image_ptr->image); + get<2>(images).push_back(cv_depth_ptr->image); + get<3>(images).push_back(i); + + } + + return images; + } + void test_compute_features(HistCloudT::Ptr& features, CloudT::Ptr& keypoints, CloudT::Ptr& cloud, NormalCloudT::Ptr& normals, bool do_visualize = false, bool is_query = false) { @@ -357,6 +420,7 @@ class retrieval_node { } results.first.resize(counter); + // the sweep paths are of the form /path/to/room.xml, which do not exist for mongodb results tie(retrieved_clouds, sweep_paths) = benchmark_retrieval::load_retrieved_clouds(results.first); cout << "Query cloud size: " << cloud->size() << endl; @@ -369,9 +433,14 @@ class retrieval_node { for (auto s : results.first) { boost::filesystem::path segment_path = s.first; string name = segment_path.stem().string(); - size_t last_index = name.find_last_not_of("0123456789"); - int index = stoi(name.substr(last_index + 1)); - indices.push_back(index); + if (name == "surfel_map") { // for the mongodb results + indices.push_back(-1); + } + else { // for the metric map results + size_t last_index = name.find_last_not_of("0123456789"); + int index = stoi(name.substr(last_index + 1)); + indices.push_back(index); + } scores.push_back(s.second.score); } @@ -391,14 +460,20 @@ class retrieval_node { K << 540.0f, 0.0f, 320.0f, 0.0f, 540.0f, 240.0f, 0.0f, 0.0f, 1.0f; for (int i = 0; i < retrieved_clouds.size(); ++i) { vector inds; - auto sweep_data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); vector > sweep_transforms; - for (tf::StampedTransform& t : sweep_data.vIntermediateRoomCloudTransformsRegistered) { - Eigen::Affine3d e; - tf::transformTFToEigen(t, e); - sweep_transforms.push_back(e.inverse().matrix().cast()); + if (indices[i] == -1) { // special case for mongodb result + tie(masks[i], images[i], depths[i], inds) = generate_images_for_mongodb_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms); + } + else { + auto sweep_data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); + for (tf::StampedTransform& t : sweep_data.vIntermediateRoomCloudTransformsRegistered) { + Eigen::Affine3d e; + tf::transformTFToEigen(t, e); + sweep_transforms.push_back(e.inverse().matrix().cast()); + } + tie(masks[i], images[i], depths[i], inds) = generate_images_for_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms); + } - tie(masks[i], images[i], depths[i], inds) = generate_images_for_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms); for (int j = 0; j < inds.size(); ++j) { paths[i].push_back(sweep_paths[i].string() + " " + to_string(inds[j])); } @@ -483,10 +558,14 @@ class retrieval_node { for (auto s : results.first) { boost::filesystem::path segment_path = s.first; string name = segment_path.stem().string(); - size_t last_index = name.find_last_not_of("0123456789"); - int index = stoi(name.substr(last_index + 1)); - indices.push_back(index); - // indices.push_back(s.second.index); // global index in the vocabulary + if (name == "surfel_map") { // for the mongodb results + indices.push_back(-1); + } + else { // for the metric map results + size_t last_index = name.find_last_not_of("0123456789"); + int index = stoi(name.substr(last_index + 1)); + indices.push_back(index); + } scores.push_back(s.second.score); } @@ -494,24 +573,26 @@ class retrieval_node { vector > masks(retrieved_clouds.size()); vector > images(retrieved_clouds.size()); vector > depths(retrieved_clouds.size()); - vector > paths(retrieved_clouds.size()); + vector > paths(retrieved_clouds.size()); for (int i = 0; i < retrieved_clouds.size(); ++i) { vector inds; - auto sweep_data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); vector > sweep_transforms; - for (tf::StampedTransform& t : sweep_data.vIntermediateRoomCloudTransformsRegistered) { - Eigen::Affine3d e; - tf::transformTFToEigen(t, e); - sweep_transforms.push_back(e.inverse().matrix().cast()); + if (indices[i] == -1) { // special case for mongodb result + tie(masks[i], images[i], depths[i], inds) = generate_images_for_mongodb_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms); + } + else { + auto sweep_data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); + for (tf::StampedTransform& t : sweep_data.vIntermediateRoomCloudTransformsRegistered) { + Eigen::Affine3d e; + tf::transformTFToEigen(t, e); + sweep_transforms.push_back(e.inverse().matrix().cast()); + } + tie(masks[i], images[i], depths[i], inds) = generate_images_for_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms); + } - tie(masks[i], images[i], depths[i], inds) = generate_images_for_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms); for (int j = 0; j < inds.size(); ++j) { paths[i].push_back(sweep_paths[i].string() + " " + to_string(inds[j])); } - - //Eigen::Affine3d AT; - //tf::transformTFToEigen(sweep_data.vIntermediateRoomCloudTransforms[0], AT); - //pcl::transformPointCloud(*retrieved_clouds[i], *retrieved_clouds[i], AT); } //cv::Mat full_query_image = benchmark_retrieval::sweep_get_rgb_at(sweep_xml, scan_index); From a8bab98b5b39362879b3c8c043731ebfe6e106b3 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Wed, 29 Jun 2016 23:58:19 +0200 Subject: [PATCH 010/255] The last thing that probably needs some love is the visualization --- .../src/quasimodo_retrieval_node.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index 28db9460..d32469ea 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -405,6 +405,11 @@ class retrieval_node { // This is just to make sure that we have valid results even when some meta rooms have been deleted size_t counter = 0; while (counter < number_query) { + // assume that everything in mongodb is kept + if (results.first[counter].first.stem().string() == "surfel_map") { + ++counter; + continue; + } if (!boost::filesystem::exists(results.first[counter].first)) { auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair::result_type>& res) { return boost::filesystem::exists(res.first); @@ -527,6 +532,11 @@ class retrieval_node { // This is just to make sure that we have valid results even when some meta rooms have been deleted size_t counter = 0; while (counter < number_query) { + // assume that everything in mongodb is kept + if (results.first[counter].first.stem().string() == "surfel_map") { + ++counter; + continue; + } if (!boost::filesystem::exists(results.first[counter].first)) { auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair::result_type>& v) { return boost::filesystem::exists(v.first); From 1c489de45f3d22df7e1a74ce4bbf16968d85725c Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Thu, 30 Jun 2016 17:43:40 +0200 Subject: [PATCH 011/255] Object search script actually running --- .../scripts/retrieve_object_search.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) mode change 100644 => 100755 quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py diff --git a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py old mode 100644 new mode 100755 index 3442806f..d3824d36 --- a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py +++ b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py @@ -6,6 +6,7 @@ #from world_modeling.srv import * from cv_bridge import CvBridge, CvBridgeError import cv2 +import numpy as np # SOMA2 stuff from soma2_msgs.msg import SOMA2Object @@ -22,8 +23,7 @@ from quasimodo_msgs.msg import retrieval_query from geometry_msgs.msg import Pose import time - -UPDATE_INT_MINUTES = 100000.0 +import std_msgs pub = () @@ -32,7 +32,7 @@ def retrieval_callback(object_id): # This hardcoding is no good! world_model = World(server_host='localhost',server_port=62345) print(object_id) - wo = world_model.get_object(object_id) + wo = world_model.get_object(object_id.data) transforms = [] #wo._poses images = [] @@ -42,12 +42,12 @@ def retrieval_callback(object_id): req = mask_pointcloudsRequest() for obs, pose in zip(wo._observations, wo._poses): - images.append(fo.get_message("/head_xtion/rgb/image_rect_color")) + images.append(obs.get_message("/head_xtion/rgb/image_rect_color")) depthmat = np.zeros((480,640,1), np.uint16) # have to be careful here, Johan has a different encoding depths.append(CvBridge().cv2_to_imgmsg(depthmat, "mono16")) - masks.append(fo.get_message("rgb_mask")) - cameras = fo.get_message("/head_xtion/depth_registered/sw_registered/camera_info") + masks.append(obs.get_message("rgb_mask")) + cameras.append(obs.get_message("/head_xtion/depth_registered/sw_registered/camera_info")) req.clouds.append(obs.get_message("/head_xtion/depth_registered/points")) req.masks.append(obs.get_message("rgb_mask")) @@ -99,6 +99,6 @@ def retrieval_callback(object_id): if __name__ == '__main__': rospy.init_node('retrieve_object_search', anonymous = False) - pub = rospy.Publisher("/models/query", retrieval_query) - sub = rospy.Subscriber("/models/mongodb_query", std_msgs.String, callback=retrieval_callback) + pub = rospy.Publisher("/models/query", data_class=retrieval_query, queue_size=None) + sub = rospy.Subscriber("/models/mongodb_query", std_msgs.msg.String, callback=retrieval_callback) rospy.spin() From cefaa82ebf18794d3afabf8179340f9b78923080 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Thu, 30 Jun 2016 21:08:14 +0200 Subject: [PATCH 012/255] Some debug output --- .../include/dynamic_object_retrieval/dynamic_retrieval.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index c7e81184..caaeedc6 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -55,7 +55,8 @@ std::vector get_retrieved_paths(const std::vector= offset) { @@ -63,6 +64,7 @@ std::vector get_retrieved_paths(const std::vector strs; From aa0cfbeaf25d098af5400ef36b7c02881cead473 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Fri, 1 Jul 2016 12:01:08 +0200 Subject: [PATCH 013/255] Added a verbose option --- .../dynamic_retrieval.h | 51 +++++++++++++++---- 1 file changed, 42 insertions(+), 9 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index 6ddfe10d..4158cf03 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -40,18 +40,35 @@ struct path_result > { }; template -std::vector get_retrieved_paths(const std::vector& scores, const vocabulary_summary& summary) +std::vector get_retrieved_paths(const std::vector& scores, const vocabulary_summary& summary, bool verbose = false) { + if (verbose) { + std::cout << "Entering get_retrieved_paths" << std::endl; + } + // here we'll make use of the info saved in the data_summaries // maybe we should cache these as well as they might get big? data_summary noise_summary; + if (verbose) { + std::cout << "Loading noise data summary..." << std::endl; + } noise_summary.load(boost::filesystem::path(summary.noise_data_path)); data_summary annotated_summary; + if (verbose) { + std::cout << "Loading annotated data summary..." << std::endl; + } annotated_summary.load(boost::filesystem::path(summary.annotated_data_path)); + if (verbose) { + std::cout << "Number of results: " << scores.size() << std::endl; + std::cout << "Number of noise segments: " << noise_summary.index_convex_segment_paths.size() << std::endl; + std::cout << "Number of annotated segments: " << noise_summary.index_convex_segment_paths.size() << std::endl; + } + std::vector retrieved_paths; size_t offset = summary.nbr_noise_segments; for (IndexT s : scores) { + std::cout << "Segment vocabulary index: " << s.index << std::endl; // TODO: vt index is not correct for grouped_vocabulary if (s.index < offset) { retrieved_paths.push_back(boost::filesystem::path(noise_summary.index_convex_segment_paths[s.index])); @@ -61,23 +78,36 @@ std::vector get_retrieved_paths(const std::vector get_retrieved_paths(const std::vector& scores, const vocabulary_summary& summary); std::vector::result_type> > -get_retrieved_path_scores(const std::vector::result_type>& scores, const vocabulary_summary& summary) +get_retrieved_path_scores(const std::vector::result_type>& scores, const vocabulary_summary& summary, bool verbose = false) { - std::vector paths = get_retrieved_paths(scores, summary); + if (verbose) { + std::cout << "Entering get_retrieved_path_scores" << std::endl; + } + + std::vector paths = get_retrieved_paths(scores, summary, verbose); std::vector::result_type> > path_scores; for (auto tup : dynamic_object_retrieval::zip(paths, scores)) { path_scores.push_back(std::make_pair(boost::get<0>(tup), boost::get<1>(tup))); } + + if (verbose) { + std::cout << "Exiting get_retrieved_path_scores" << std::endl; + } + return path_scores; } std::vector, grouped_vocabulary_tree::result_type> > -get_retrieved_path_scores(const std::vector::result_type>& scores, const vocabulary_summary& summary) +get_retrieved_path_scores(const std::vector::result_type>& scores, const vocabulary_summary& summary, bool verbose = false) { // OK, now we need to get the paths for all of the stuff within the scans instead, so basically // we need to get a subsegment_sweep__path_iterator and add all of the path corresponding to groups @@ -134,10 +164,13 @@ template std::vector::type, typename VocabularyT::result_type> > query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, VocabularyT& vt, const boost::filesystem::path& vocabulary_path, - const vocabulary_summary& summary) + const vocabulary_summary& summary, bool verbose = false) { // we need to cache the vt if we are to do this multiple times if (vt.empty()) { + if (verbose) { + std::cout << "No vocabulary loaded, reading vocabulary..." << std::endl; + } load_vocabulary(vt, vocabulary_path); vt.set_min_match_depth(3); vt.compute_normalizing_constants(); @@ -147,7 +180,7 @@ query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, VocabularyT& vt, std::vector scores; vt.query_vocabulary(scores, features, nbr_query); - return get_retrieved_path_scores(scores, summary); + return get_retrieved_path_scores(scores, summary, verbose); } // OK, the solution here is to turn the groups into the path scores @@ -155,7 +188,7 @@ template <> std::vector >::type, typename grouped_vocabulary_tree::result_type> > query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, grouped_vocabulary_tree& vt, const boost::filesystem::path& vocabulary_path, - const vocabulary_summary& summary) + const vocabulary_summary& summary, bool verbose) { // we need to cache the vt if we are to do this multiple times if (vt.empty()) { @@ -480,7 +513,7 @@ std::pair::type, typenam std::vector::type, typename VocabularyT::result_type> > > query_reweight_vocabulary(VocabularyT& vt, HistCloudT::Ptr& query_features, size_t nbr_query, const boost::filesystem::path& vocabulary_path, - const vocabulary_summary& summary) + const vocabulary_summary& summary, bool verbose = false) { using result_type = std::vector::type, typename VocabularyT::result_type> >; @@ -495,7 +528,7 @@ query_reweight_vocabulary(VocabularyT& vt, HistCloudT::Ptr& query_features, std::cout << "Querying vocabulary..." << std::endl; TICK("query_vocabulary"); - result_type retrieved_paths = query_vocabulary(query_features, nbr_query, vt, vocabulary_path, summary); + result_type retrieved_paths = query_vocabulary(query_features, nbr_query, vt, vocabulary_path, summary, verbose); TOCK("query_vocabulary"); return make_pair(retrieved_paths, result_type()); From 1f1246d8945117c4b464e060cc69990778b4dff1 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 1 Jul 2016 14:05:43 +0200 Subject: [PATCH 014/255] Force added all of JOhans changes --- quasimodo/quasimodo_brain/CMakeLists.txt | 13 + quasimodo/quasimodo_brain/package.xml | 2 + quasimodo/quasimodo_brain/src/modelserver.cpp | 542 +++- .../src/preload_object_data.cpp | 687 +++-- .../quasimodo_brain/src/robot_listener.cpp | 6 + quasimodo/quasimodo_brain/src/test_align2.cpp | 303 ++ .../quasimodo_brain/src/test_segment.cpp | 663 +++++ quasimodo/quasimodo_brain/src/velodyne2.cpp | 375 ++- quasimodo/quasimodo_brain/src/voPCD.cpp | 88 + quasimodo/quasimodo_models/CMakeLists.txt | 10 +- .../quasimodo_models/include/core/RGBDFrame.h | 5 + .../quasimodo_models/include/core/Util.h | 3 +- .../quasimodo_models/include/model/Model.h | 48 +- .../include/modelupdater/ModelUpdater.h | 40 +- .../include/registration/MassRegistration.h | 10 + .../registration/MassRegistrationPPR.h | 33 +- .../registration/MassRegistrationPPR2.h | 162 + .../include/registration/Registration.h | 1 + .../include/registration/RegistrationRandom.h | 2 + .../registration/RegistrationRefinement.h | 3 +- .../src/core/2d/convolution.h | 159 + quasimodo/quasimodo_models/src/core/2d/edge.h | 306 ++ .../src/core/2d/impl/convolution.hpp | 153 + .../src/core/2d/impl/edge.hpp | 494 ++++ .../src/core/2d/impl/kernel.hpp | 328 +++ .../src/core/2d/impl/keypoint.hpp | 247 ++ .../src/core/2d/impl/morphology.hpp | 380 +++ .../quasimodo_models/src/core/2d/kernel.h | 244 ++ .../quasimodo_models/src/core/2d/keypoint.h | 75 + .../quasimodo_models/src/core/2d/morphology.h | 200 ++ .../quasimodo_models/src/core/RGBDFrame.cpp | 130 +- quasimodo/quasimodo_models/src/core/Util.cpp | 6 +- .../src/core/organized_edge_detection.h | 431 +++ .../src/core/organized_edge_detection.hpp | 354 +++ .../quasimodo_models/src/model/Model.cpp | 296 +- .../quasimodo_models/src/model/ModelMask.cpp | 57 +- .../src/modelupdater/ModelUpdater.cpp | 1177 +++++++- .../modelupdater/ModelUpdaterBasicFuse.cpp | 413 +-- .../src/registration/MassRegistration.cpp | 54 +- .../src/registration/MassRegistrationPPR.cpp | 2368 ++++++++------- .../src/registration/MassRegistrationPPR2.cpp | 2595 +++++++++++++++++ .../src/registration/RegistrationRandom.cpp | 240 +- .../registration/RegistrationRefinement.cpp | 45 +- .../DistanceWeightFunction2PPR2.cpp | 7 +- 44 files changed, 11804 insertions(+), 1951 deletions(-) create mode 100644 quasimodo/quasimodo_brain/src/test_align2.cpp create mode 100644 quasimodo/quasimodo_brain/src/test_segment.cpp create mode 100644 quasimodo/quasimodo_brain/src/voPCD.cpp create mode 100644 quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h create mode 100644 quasimodo/quasimodo_models/src/core/2d/convolution.h create mode 100644 quasimodo/quasimodo_models/src/core/2d/edge.h create mode 100644 quasimodo/quasimodo_models/src/core/2d/impl/convolution.hpp create mode 100644 quasimodo/quasimodo_models/src/core/2d/impl/edge.hpp create mode 100644 quasimodo/quasimodo_models/src/core/2d/impl/kernel.hpp create mode 100644 quasimodo/quasimodo_models/src/core/2d/impl/keypoint.hpp create mode 100644 quasimodo/quasimodo_models/src/core/2d/impl/morphology.hpp create mode 100644 quasimodo/quasimodo_models/src/core/2d/kernel.h create mode 100644 quasimodo/quasimodo_models/src/core/2d/keypoint.h create mode 100644 quasimodo/quasimodo_models/src/core/2d/morphology.h create mode 100644 quasimodo/quasimodo_models/src/core/organized_edge_detection.h create mode 100644 quasimodo/quasimodo_models/src/core/organized_edge_detection.hpp create mode 100644 quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index b5d2e4dd..0a6527da 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -113,6 +113,11 @@ add_executable( massregPCD src/massregPCD.cpp) add_dependencies( massregPCD roscpp quasimodo_msgs_generate_messages_cpp) target_link_libraries( massregPCD quasimodo_ModelDatabase quasimodo_ModelUpdater image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +add_executable( voPCD src/voPCD.cpp) +add_dependencies( voPCD roscpp quasimodo_msgs_generate_messages_cpp) +target_link_libraries( voPCD quasimodo_ModelDatabase quasimodo_ModelUpdater image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + + add_executable( velodyne2 src/velodyne2.cpp) add_dependencies( velodyne2 roscpp quasimodo_msgs_generate_messages_cpp) target_link_libraries( velodyne2 quasimodo_ModelDatabase quasimodo_ModelUpdater image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) @@ -121,6 +126,14 @@ add_executable( MeshTest src/MeshTest.cpp) add_dependencies( MeshTest roscpp quasimodo_msgs_generate_messages_cpp) target_link_libraries( MeshTest quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +add_executable( test_align2 src/test_align2.cpp) +add_dependencies( test_align2 roscpp quasimodo_msgs_generate_messages_cpp) +target_link_libraries( test_align2 quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + +add_executable( test_segment src/test_segment.cpp) +add_dependencies( test_segment roscpp quasimodo_msgs_generate_messages_cpp) +target_link_libraries( test_segment quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + ############# ## Install ## ############# diff --git a/quasimodo/quasimodo_brain/package.xml b/quasimodo/quasimodo_brain/package.xml index 0f0281f1..a7b8ed25 100644 --- a/quasimodo/quasimodo_brain/package.xml +++ b/quasimodo/quasimodo_brain/package.xml @@ -56,6 +56,8 @@ message_runtime eigen_conversions cv_bridge + semantic_map + semantic_map quasimodo_models quasimodo_msgs metaroom_xml_parser diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 10b95d35..8eb31494 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -12,6 +12,7 @@ #include "quasimodo_msgs/index_frame.h" #include "quasimodo_msgs/fuse_models.h" #include "quasimodo_msgs/get_model.h" +#include "quasimodo_msgs/recognize_model.h" #include "quasimodo_msgs/retrieval_query_result.h" #include "quasimodo_msgs/retrieval_query.h" #include "quasimodo_msgs/retrieval_result.h" @@ -58,7 +59,9 @@ std::map frames; std::map models; std::map updaters; -reglib::RegistrationRandom * registration; + +std::set framekeys; +reglib::RegistrationRandom * registration; ModelDatabase * modeldatabase; std::string savepath = "."; @@ -75,12 +78,14 @@ ros::Publisher chatter_pub; ros::ServiceClient soma2add; -double occlusion_penalty = 15; -double massreg_timeout = 60; +double occlusion_penalty = 10; +double massreg_timeout = 120; bool run_search = false; double search_timeout = 30; +int sweepid_counter = 0; + bool myfunction (reglib::Model * i,reglib::Model * j) { return i->frames.size() > j->frames.size(); } double getTime(){ @@ -174,6 +179,7 @@ void retrievalCallback(const quasimodo_msgs::retrieval_query_result & qr){ int savecounter = 0; void show_sorted(){ + return; if(!visualization){return;} std::vector results; for(unsigned int i = 0; i < modeldatabase->models.size(); i++){results.push_back(modeldatabase->models[i]);} @@ -216,17 +222,16 @@ void show_sorted(){ //viewer->spinOnce(); } -quasimodo_msgs::model getModelMSG(reglib::Model * model){ - quasimodo_msgs::model msg; - msg.model_id = model->id; - msg.local_poses.resize(model->relativeposes.size()); - msg.frames.resize(model->frames.size()); - msg.masks.resize(model->modelmasks.size()); +void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp = Eigen::Affine3d::Identity()){ + int startsize = msg.local_poses.size(); + msg.local_poses.resize(startsize+model->relativeposes.size()); + msg.frames.resize(startsize+model->frames.size()); + msg.masks.resize(startsize+model->modelmasks.size()); for(unsigned int i = 0; i < model->relativeposes.size(); i++){ geometry_msgs::Pose pose1; - tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i]), pose1); + tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i])*rp, pose1); geometry_msgs::Pose pose2; - tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose), pose2); + tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose)*rp, pose2); cv_bridge::CvImage rgbBridgeImage; rgbBridgeImage.image = model->frames[i]->rgb; rgbBridgeImage.encoding = "bgr8"; @@ -236,17 +241,71 @@ quasimodo_msgs::model getModelMSG(reglib::Model * model){ cv_bridge::CvImage maskBridgeImage; maskBridgeImage.image = model->modelmasks[i]->getMask(); maskBridgeImage.encoding = "mono8"; - msg.local_poses[i] = pose1; - msg.frames[i].capture_time = ros::Time(); - msg.frames[i].pose = pose2; - msg.frames[i].frame_id = model->frames[i]->id; - msg.frames[i].rgb = *(rgbBridgeImage.toImageMsg()); - msg.frames[i].depth = *(depthBridgeImage.toImageMsg()); - msg.masks[i] = *(maskBridgeImage.toImageMsg());//getMask() + msg.local_poses[startsize+i] = pose1; + msg.frames[startsize+i].capture_time = ros::Time(); + msg.frames[startsize+i].pose = pose2; + msg.frames[startsize+i].frame_id = model->frames[i]->id; + msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); + msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); + msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg());//getMask() + } + for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ + addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp); } +} + +quasimodo_msgs::model getModelMSG(reglib::Model * model){ + quasimodo_msgs::model msg; + msg.model_id = model->id; + addToModelMSG(msg,model); + + return msg; } +reglib::Model * getModelFromMSG(quasimodo_msgs::model msg){ + reglib::Model * model = new reglib::Model(); + + for(unsigned int i = 0; i < msg.local_poses.size(); i++){ + sensor_msgs::CameraInfo camera = msg.frames[i].camera; + ros::Time capture_time = msg.frames[i].capture_time; + geometry_msgs::Pose pose = msg.frames[i].pose; + + cv_bridge::CvImagePtr rgb_ptr; + try{ rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat rgb = rgb_ptr->image; + + cv_bridge::CvImagePtr depth_ptr; + try{ depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat depth = depth_ptr->image; + + Eigen::Affine3d epose; + tf::poseMsgToEigen(pose, epose); + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); + model->frames.push_back(frame); + + geometry_msgs::Pose pose1 = msg.local_poses[i]; + Eigen::Affine3d epose1; + tf::poseMsgToEigen(pose1, epose1); + model->relativeposes.push_back(epose1.matrix()); + + cv_bridge::CvImagePtr mask_ptr; + try{ mask_ptr = cv_bridge::toCvCopy(msg.masks[i], sensor_msgs::image_encodings::MONO8);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat mask = mask_ptr->image; + + model->modelmasks.push_back(new reglib::ModelMask(mask)); + model->modelmasks.back()->sweepid = sweepid_counter; + } + model->recomputeModelPoints(); + sweepid_counter++; + + return model; +} + std::vector getSOMA2ObjectMSGs(reglib::Model * model){ std::vector msgs; for(unsigned int i = 0; i < model->frames.size(); i++){ @@ -360,6 +419,94 @@ void publishModel(reglib::Model * model){ // } } +bool recognizeModel(quasimodo_msgs::recognize_model::Request & req, quasimodo_msgs::recognize_model::Response & res){ + if(modeldatabase->models.size() == 0){res.score = -1;return true;} + reglib::Model * model = getModelFromMSG(req.inputmodel); + + double best = 0; + int best_ind = 0; + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + reglib::Model * model2 = modeldatabase->models[i]; + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model2, reg); + mu->occlusion_penalty = occlusion_penalty; + mu->massreg_timeout = massreg_timeout; + mu->viewer = viewer; + reg->visualizationLvl = 0; + reglib::FusionResults fr = mu->registerModel(model); + + if(fr.score > 100){ + fr.guess = fr.guess.inverse(); + + Eigen::Matrix4d pose = fr.guess; + std::vector current_poses; + std::vector current_frames; + std::vector current_modelmasks; + + for(unsigned int i = 0; i < model->frames.size(); i++){ + current_poses.push_back( model->relativeposes[i]); + current_frames.push_back( model->frames[i]); + current_modelmasks.push_back( model->modelmasks[i]); + } + + for(unsigned int i = 0; i < model2->frames.size(); i++){ + current_poses.push_back( pose * model2->relativeposes[i]); + current_frames.push_back( model2->frames[i]); + current_modelmasks.push_back( model2->modelmasks[i]); + } + + std::vector > ocs = mu->getOcclusionScores(current_poses, current_frames,current_modelmasks,false); + std::vector > scores = mu->getScores(ocs); + + + unsigned int frames1 = model->frames.size(); + unsigned int frames2 = model2->frames.size(); + double sumscore1 = 0; + for(unsigned int i = 0; i < frames1; i++){ + for(unsigned int j = 0; j < frames1; j++){ + sumscore1 += scores[i][j]; + } + } + + double sumscore2 = 0; + for(unsigned int i = 0; i < frames2; i++){ + for(unsigned int j = 0; j < frames2; j++){ + sumscore2 += scores[i+frames1][j+frames1]; + } + } + + double sumscore = 0; + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + sumscore += scores[i][j]; + } + } + + double improvement = sumscore-sumscore1-sumscore2; + if(i == 0 || improvement > best){ + best_ind = i; + best = improvement; + } + } + + delete mu; + delete reg; + } + + for(unsigned int i = 0; i < model->frames.size(); i++){ + delete model->frames[i]; + delete model->modelmasks[i]; + } + delete model; + + res.score = best; + res.outputmodel = getModelMSG(modeldatabase->models[best_ind]); + //int model_id = req.model_id; + //reglib::Model * model = models[model_id]; + //res.model = getModelMSG(model); + return true; +} + bool getModel(quasimodo_msgs::get_model::Request & req, quasimodo_msgs::get_model::Response & res){ int model_id = req.model_id; reglib::Model * model = models[model_id]; @@ -368,7 +515,6 @@ bool getModel(quasimodo_msgs::get_model::Request & req, quasimodo_msgs::get_mod } bool indexFrame(quasimodo_msgs::index_frame::Request & req, quasimodo_msgs::index_frame::Response & res){ - //printf("indexFrame\n"); sensor_msgs::CameraInfo camera = req.frame.camera; ros::Time capture_time = req.frame.capture_time; geometry_msgs::Pose pose = req.frame.pose; @@ -386,9 +532,13 @@ bool indexFrame(quasimodo_msgs::index_frame::Request & req, quasimodo_msgs::ind Eigen::Affine3d epose; tf::poseMsgToEigen(pose, epose); - //printf("%s LINE:%i\n",__FILE__,__LINE__); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); - //printf("%s LINE:%i\n",__FILE__,__LINE__); + reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS + cam->fx = camera.K[0]; + cam->fy = camera.K[4]; + cam->cx = camera.K[2]; + cam->cy = camera.K[5]; + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); frames[frame->id] = frame; res.frame_id = frame->id; return true; @@ -444,16 +594,16 @@ void call_from_thread(int i) { int current_model_update = 0; void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, bool deleteIfFail = false){ printf("addToDB %i %i\n",int(add),int(deleteIfFail)); - if(add){ - if(model->frames.size() > 2){ + if(add){ + if(model->submodels.size() > 2){ reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model, 0); mu->occlusion_penalty = occlusion_penalty; mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; reg->visualizationLvl = 0; - mu->refine(0.001,true); + mu->refine(0.001,false,0); delete mu; delete reg; } @@ -462,9 +612,11 @@ void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, b model->last_changed = ++current_model_update; // show_sorted(); } - +show_sorted(); mod = model; - res = modeldatabase->search(model,150); + res = modeldatabase->search(model,3); + + printf("results found: %i\n",res.size()); fr_res.resize(res.size()); if(res.size() == 0){ @@ -487,7 +639,7 @@ void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, b reglib::Model * model2 = res[i]; reglib::FusionResults fr = fr_res[i]; if(fr.score > 100){ - fr.guess = fr.guess.inverse(); + //fr.guess = fr.guess.inverse(); fr2merge.push_back(fr); models2merge.push_back(model2); printf("%i could be registered\n",i); @@ -523,20 +675,20 @@ void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, b if(ud.deleted_models.size() > 0){changed = true; break;} } }else{ + printf("TIME TO ITERATE THROUGH RESULTS\n"); for(unsigned int i = 0; i < res.size(); i++){ + printf("TESTING %i\n",i); reglib::Model * model2 = res[i]; reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model2, reg); mu->occlusion_penalty = occlusion_penalty; mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; - reg->visualizationLvl = 0; + reg->visualizationLvl = 2; reglib::FusionResults fr = mu->registerModel(model); if(fr.score > 100){ - fr.guess = fr.guess.inverse(); - - reglib::UpdatedModels ud = mu->fuseData(&fr, model, model2); + reglib::UpdatedModels ud = mu->fuseData(&fr, model2, model); printf("merge %i to %i\n",int(model->id),int(model2->id)); printf("new_models: %i\n",int(ud.new_models.size())); printf("updated_models: %i\n",int(ud.updated_models.size())); @@ -549,11 +701,19 @@ void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, b database->remove(ud.deleted_models[j]); delete ud.deleted_models[j]; } - if(ud.deleted_models.size() > 0){changed = true; break;} + if(ud.deleted_models.size() > 0){ + changed = true; + delete mu; + delete reg; + printf("DONE TESTING %i\n",i); + break; + } } delete mu; delete reg; + + printf("DONE TESTING %i\n",i); } } @@ -586,8 +746,11 @@ void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, b if(deleteIfFail){ if(!changed){ printf("didnt manage to integrate searchresult\n"); + database->remove(model); for(unsigned int i = 0; i < model->frames.size(); i++){ + std::string key = model->frames[i]->keyval; + if(framekeys.count(key) != 0){framekeys.erase(key);} delete model->frames[i]; delete model->modelmasks[i]; } @@ -601,104 +764,164 @@ void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, b bool nextNew = true; reglib::Model * newmodel = 0; -int sweepid_counter = 0; //std::vector newmasks; std::vector allmasks; int modaddcount = 0; bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_msgs::model_from_frame::Response & res){ - printf("======================================\nmodelFromFrame\n======================================\n"); + //printf("======================================\nmodelFromFrame\n======================================\n"); uint64 frame_id = req.frame_id; uint64 isnewmodel = req.isnewmodel; - printf("%i and %i\n",int(frame_id),int(isnewmodel)); + //printf("%i and %i\n",int(frame_id),int(isnewmodel)); + + printf("modelFromFrame %i and %i\n",int(frame_id),int(isnewmodel)); + cv_bridge::CvImagePtr mask_ptr; try{ mask_ptr = cv_bridge::toCvCopy(req.mask, sensor_msgs::image_encodings::MONO8);} catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());return false;} cv::Mat mask = mask_ptr->image; allmasks.push_back(mask); - std::cout << frames[frame_id]->pose << std::endl << std::endl; + //std::cout << frames[frame_id]->pose << std::endl << std::endl; - cv::Mat fullmask; - fullmask.create(480,640,CV_8UC1); - unsigned char * maskdata = (unsigned char *)fullmask.data; - for(unsigned int j = 0; j < 640*480; j++){maskdata[j] = 255;} if(newmodel == 0){ newmodel = new reglib::Model(frames[frame_id],mask); - modeldatabase->add(newmodel); - // newmodel->masks.back() = fullmask; - // newmodel->recomputeModelPoints(); + //modeldatabase->add(newmodel); }else{ - // reglib::Model * inputmodel = new reglib::Model(frames[frame_id],mask); - // inputmodel->masks.back() = fullmask; - // inputmodel->recomputeModelPoints(); - - // reglib::RegistrationRefinement * reg = new reglib::RegistrationRefinement(); - // //RegistrationRefinement - // reg->target_points = 3000; - // reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( inputmodel, reg); - // mu->viewer = viewer; - // reg->visualizationLvl = 4; - // reglib::FusionResults fr = mu->registerModel(newmodel,(newmodel->frames.front()->pose.inverse() * frames[frame_id]->pose).inverse()); - // fr.guess = fr.guess.inverse(); - // delete inputmodel; - // delete mu; - // delete reg; - newmodel->frames.push_back(frames[frame_id]); - //newmodel->masks.push_back(mask); newmodel->relativeposes.push_back(newmodel->frames.front()->pose.inverse() * frames[frame_id]->pose); - //newmodel->relativeposes.push_back(frames[frame_id]->pose); - //newmodel->relativeposes.push_back(newmodel->frames.front()->pose.inverse() * fr.guess); newmodel->modelmasks.push_back(new reglib::ModelMask(mask)); newmodel->recomputeModelPoints(); - } newmodel->modelmasks.back()->sweepid = sweepid_counter; - //printf("LINE: %i\n",__LINE__); - - //show_sorted(); res.model_id = newmodel->id; - if(isnewmodel != 0){ - //newmodel->masks = newmasks; - //newmasks.clear(); + int current_model_update_before = current_model_update; newmodel->recomputeModelPoints(); - //show_sorted(); + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( newmodel, reg); mu->occlusion_penalty = occlusion_penalty; mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; - // reg->visualizationLvl = 0; - mu->refine(0.01,true); - mu->recomputeScores(); - delete mu; - delete reg; - int current_model_update_before = current_model_update; - newmodel->recomputeModelPoints(); + +newmodel->print(); + mu->makeInitialSetup(); +newmodel->print(); +delete mu; +delete reg; + + reglib::Model * newmodelHolder = new reglib::Model(); + newmodelHolder->submodels.push_back(newmodel); + newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + newmodelHolder->last_changed = ++current_model_update; + newmodelHolder->recomputeModelPoints(); + + modeldatabase->add(newmodelHolder); + addToDB(modeldatabase, newmodelHolder,false); + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){modeldatabase->models[i]->showHistory(viewer);} + show_sorted(); + +//exit(0); + + +/* +//SIMPLE TEST +reglib::Model * testmodel = new reglib::Model(); +for(int i = 0; i < newmodel->frames.size();i++){ + printf("%i / %i\n",i+1,newmodel->frames.size()); + reglib::Model * tmp = new reglib::Model(); + tmp->frames.push_back(newmodel->frames[i]); + tmp->modelmasks.push_back(newmodel->modelmasks[i]); + tmp->relativeposes.push_back(Eigen::Matrix4d::Identity()); + tmp->recomputeModelPoints(); + + std::vector cp; + std::vector cf; + std::vector cm; + for(unsigned int i = 0; i < tmp->relativeposes.size(); i++){ + cp.push_back(tmp->relativeposes[i]); + cf.push_back(tmp->frames[i]); + cm.push_back(tmp->modelmasks[i]); + } + mu->getGoodCompareFrames(cp,cf,cm); + tmp->rep_relativeposes = cp; + tmp->rep_frames = cf; + tmp->rep_modelmasks = cm; + +// pcl::PointCloud::Ptr cloud = tmp->getPCLnormalcloud(1,false); +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); +// viewer->spin(); + + testmodel->submodels.push_back(tmp); + testmodel->submodels_relativeposes.push_back(newmodel->relativeposes[i]); +} + +testmodel->recomputeModelPoints(); +{ + pcl::PointCloud::Ptr cloud = testmodel->getPCLnormalcloud(1,false); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); +} + +std::vector testmodels; +std::vector testrps; +mu->addModelsToVector(testmodels,testrps,testmodel,Eigen::Matrix4d::Identity()); + +printf("testmodels: %i\n",testmodels.size()); +std::vector > testocs = mu->computeOcclusionScore(testmodels,testrps); +std::vector > scores = mu->getScores(testocs); +std::vector partition = mu->getPartition(scores,2,5,2); + +for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + if(scores[i][j] > 0){printf(" ");} + printf("%5.5f ",0.0001*scores[i][j]); + } + printf("\n"); +} + +printf("partition"); +for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} +printf("\n"); +*/ + +//exit(0); +/* +vector models; +vector rps; + +// models.push_back(model); +// models.push_back(model2); +// rps.push_back(Eigen::Matrix4d::Identity()); +// rps.push_back(pose); + +addModelsToVector(models,rps,model,Eigen::Matrix4d::Identity()); +unsigned int nr_models1 = models.size(); +addModelsToVector(models,rps,model2,pose); +vector > ocs = computeOcclusionScore(models,rps); +*/ +/* + + //newmodel->recomputeModelPoints(); + + newmodel->last_changed = ++current_model_update; newmodel->print(); +//exit(0); addToDB(modeldatabase, newmodel,false); //if(modaddcount % 1 == 0){show_sorted();} - show_sorted(); + //show_sorted(); publishDatabasePCD(); modaddcount++; - dumpDatabase(savepath); - - // cameras[0]->save("./camera0"); - // for(unsigned int m = 0; m < modeldatabase->models.size(); m++){ - // char buf [1024]; - // sprintf(buf,"./model%i",m); - // char command [1024]; - // sprintf(command,"mkdir -p %s",buf); - // system(command); - // modeldatabase->models[m]->save(std::string(buf)); - // } - //exit(0); + //dumpDatabase(savepath); +*/ for(unsigned int m = 0; run_search && m < modeldatabase->models.size(); m++){ printf("looking at: %i\n",int(modeldatabase->models[m]->last_changed)); @@ -720,7 +943,14 @@ bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_ int maxval = 0; int maxind = 0; + int dilation_size = 10; + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, + cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), + cv::Point( dilation_size, dilation_size ) ); + for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ + //framekeys + cv_bridge::CvImagePtr ret_image_ptr; try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} @@ -737,10 +967,12 @@ bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_ cv::Mat maskimage = ret_mask_ptr->image; cv::Mat depthimage = ret_depth_ptr->image; + cv::Mat erosion_dst; + cv::erode( maskimage, erosion_dst, element ); unsigned short * depthdata = (unsigned short * )depthimage.data; unsigned char * rgbdata = (unsigned char * )rgbimage.data; - unsigned char * maskdata = (unsigned char * )maskimage.data; + unsigned char * maskdata = (unsigned char * )erosion_dst.data; int count = 0; for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ @@ -755,63 +987,81 @@ bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_ //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ if(maxval > 100){//Atleast some pixels has to be in the mask... - int j = maxind; - - cv_bridge::CvImagePtr ret_image_ptr; - try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv_bridge::CvImagePtr ret_mask_ptr; - try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv_bridge::CvImagePtr ret_depth_ptr; - try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv::Mat rgbimage = ret_image_ptr->image; - cv::Mat maskimage = ret_mask_ptr->image; - cv::Mat depthimage = ret_depth_ptr->image; - for (int ii = 0; ii < depthimage.rows; ++ii) { - for (int jj = 0; jj < depthimage.cols; ++jj) { - depthimage.at(ii, jj) *= 5; + std::string key = sresult.retrieved_image_paths[i].strings[maxind]; + printf("searchresult key:%s\n",key.c_str()); + if(framekeys.count(key) == 0){ + + int j = maxind; + + cv_bridge::CvImagePtr ret_image_ptr; + try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + cv_bridge::CvImagePtr ret_mask_ptr; + try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + cv_bridge::CvImagePtr ret_depth_ptr; + try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + cv::Mat rgbimage = ret_image_ptr->image; + cv::Mat maskimage = ret_mask_ptr->image; + cv::Mat depthimage = ret_depth_ptr->image; + for (int ii = 0; ii < depthimage.rows; ++ii) { + for (int jj = 0; jj < depthimage.cols; ++jj) { + depthimage.at(ii, jj) *= 5; + } } - } -// cv::Mat overlap = rgbimage.clone(); -// unsigned short * depthdata = (unsigned short * )depthimage.data; -// unsigned char * rgbdata = (unsigned char * )rgbimage.data; -// unsigned char * maskdata = (unsigned char * )maskimage.data; -// unsigned char * overlapdata = (unsigned char * )overlap.data; -// for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ -// if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ -// overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; -// overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; -// overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; -// }else{ -// overlapdata[3*pixel+0] = 0; -// overlapdata[3*pixel+1] = 0; -// overlapdata[3*pixel+2] = 0; -// } -// } -// cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); -// cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); -// cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); -// cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); - - Eigen::Affine3d epose = Eigen::Affine3d::Identity(); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); - reglib::Model * searchmodel = new reglib::Model(frame,maskimage); - bool res = searchmodel->testFrame(0); - -// if(cv::waitKey(0) != 'n'){ + cv::Mat erosion_dst; + cv::erode( maskimage, erosion_dst, element ); + + // cv::Mat overlap = rgbimage.clone(); + // unsigned short * depthdata = (unsigned short * )depthimage.data; + // unsigned char * rgbdata = (unsigned char * )rgbimage.data; + // unsigned char * maskdata = (unsigned char * )maskimage.data; + // unsigned char * overlapdata = (unsigned char * )overlap.data; + // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ + // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ + // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; + // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; + // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; + // }else{ + // overlapdata[3*pixel+0] = 0; + // overlapdata[3*pixel+1] = 0; + // overlapdata[3*pixel+2] = 0; + // } + // } + // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); + // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); + // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); + // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); + + Eigen::Affine3d epose = Eigen::Affine3d::Identity(); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); + frame->keyval = key; + //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); + reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); + bool res = searchmodel->testFrame(0); + + reglib::Model * searchmodelHolder = new reglib::Model(); + searchmodelHolder->submodels.push_back(searchmodel); + searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + searchmodelHolder->last_changed = ++current_model_update; + searchmodelHolder->recomputeModelPoints(); + + + // if(cv::waitKey(0) != 'n'){ //Todo:: check new model is not flat or L shape printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); - addToDB(modeldatabase, searchmodel,false,true); + //addToDB(modeldatabase, searchmodel,false,true); + addToDB(modeldatabase, searchmodelHolder,false,true); show_sorted(); -// } + // } + } } } @@ -824,6 +1074,7 @@ bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_ } //exit(0); } + newmodel = 0; sweepid_counter++; } @@ -902,7 +1153,10 @@ int main(int argc, char **argv){ ros::ServiceServer service4 = n.advertiseService("get_model", getModel); ROS_INFO("Ready to add use get_model."); - soma2add = n.serviceClient( "soma2/insert_objs"); + ros::ServiceServer service5 = n.advertiseService("recognize_model", recognizeModel); + ROS_INFO("Ready to add use get_model."); + + soma2add = n.serviceClient("soma2/insert_objs"); ROS_INFO("Ready to add use soma2add."); ros::Subscriber sub = n.subscribe("/retrieval_result", 1, retrievalCallback); @@ -955,7 +1209,7 @@ int main(int argc, char **argv){ if(visualization){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); viewer->addCoordinateSystem(0.1); - viewer->setBackgroundColor(0.9,0.9,0.9); + viewer->setBackgroundColor(0.0,0.0,0.0); } ros::Duration(1.0).sleep(); diff --git a/quasimodo/quasimodo_brain/src/preload_object_data.cpp b/quasimodo/quasimodo_brain/src/preload_object_data.cpp index 05638eb5..1a0a396b 100644 --- a/quasimodo/quasimodo_brain/src/preload_object_data.cpp +++ b/quasimodo/quasimodo_brain/src/preload_object_data.cpp @@ -78,6 +78,15 @@ using LabelT = semantic_map_load_utilties::LabelledData; #include "quasimodo_msgs/fuse_models.h" #include "quasimodo_msgs/get_model.h" +#include "metaroom_xml_parser/simple_xml_parser.h" + +#include +#include + +#include "modelupdater/ModelUpdater.h" +#include "core/RGBDFrame.h" + + using namespace std; typedef pcl::PointXYZRGB PointType; @@ -98,8 +107,10 @@ std::vector< std::vector > depths; std::vector< std::vector > masks; std::vector< std::vector >tfs; std::vector< std::vector > initposes; +std::vector< std::vector< image_geometry::PinholeCameraModel > > cams; pcl::visualization::PCLVisualizer* pg; +boost::shared_ptr viewer; @@ -112,7 +123,7 @@ std::vector getRegisteredViewPoses(const std::string& poses_fil } cout<<"Loading additional view registered poses from "< getRegisteredViewPoses(const std::string& poses_fil void chatterCallback(const std_msgs::String::ConstPtr& msg) { - ROS_INFO("I heard: [%s]", msg->data.c_str()); + ROS_INFO("I heard: [%s]", msg->data.c_str()); - for(unsigned int i = 0; i < rgbs.size(); i++){ - std::vector fid; - std::vector fadded; - for(unsigned int j = 0; j < rgbs[i].size(); j++){ - printf("%i %i\n",i,j); + for(unsigned int i = 1; i < rgbs.size(); i++){ + std::vector fid; + std::vector fadded; + for(unsigned int j = 0; j < rgbs[i].size(); j++){ + printf("%i %i\n",i,j); - //cv::Mat maskimage = masks[i][j]; - cv::Mat rgbimage = rgbs[i][j]; - cv::Mat depthimage = depths[i][j]; - tf::StampedTransform tf = tfs[i][j]; - Eigen::Matrix4f m = initposes[i][j]; - geometry_msgs::TransformStamped tfstmsg; - tf::transformStampedTFToMsg (tf, tfstmsg); + //cv::Mat maskimage = masks[i][j]; + cv::Mat rgbimage = rgbs[i][j]; + cv::Mat depthimage = depths[i][j]; + tf::StampedTransform tf = tfs[i][j]; + Eigen::Matrix4f m = initposes[i][j]; + geometry_msgs::TransformStamped tfstmsg; + tf::transformStampedTFToMsg (tf, tfstmsg); - geometry_msgs::Transform tfmsg = tfstmsg.transform; + geometry_msgs::Transform tfmsg = tfstmsg.transform; - printf("start adding frame %i\n",i); + printf("start adding frame %i\n",i); Eigen::Quaternionf q = Eigen::Quaternionf(Eigen::Affine3f(m).rotation()); - geometry_msgs::Pose pose; -// pose.orientation = tfmsg.rotation; -// pose.position.x = tfmsg.translation.x; -// pose.position.y = tfmsg.translation.y; -// pose.position.z = tfmsg.translation.z; - pose.orientation.x = q.x(); - pose.orientation.y = q.y(); - pose.orientation.z = q.z(); - pose.orientation.w = q.w(); - pose.position.x = m(0,3); - pose.position.y = m(1,3); - pose.position.z = m(2,3); - - - cv_bridge::CvImage rgbBridgeImage; - rgbBridgeImage.image = rgbimage; - rgbBridgeImage.encoding = "bgr8"; - - cv_bridge::CvImage depthBridgeImage; - depthBridgeImage.image = depthimage; - depthBridgeImage.encoding = "mono16"; - - quasimodo_msgs::index_frame ifsrv; - ifsrv.request.frame.capture_time = ros::Time(); - ifsrv.request.frame.pose = pose; - ifsrv.request.frame.frame_id = -1; - ifsrv.request.frame.rgb = *(rgbBridgeImage.toImageMsg()); - ifsrv.request.frame.depth = *(depthBridgeImage.toImageMsg()); - - if (index_frame_client.call(ifsrv)){//Add frame to model server - int frame_id = ifsrv.response.frame_id; - fadded.push_back(j); - fid.push_back(frame_id); - ROS_INFO("frame_id%i", frame_id ); - }else{ROS_ERROR("Failed to call service index_frame");} - - printf("stop adding frame %i\n",i); - } - - for(unsigned int j = 0; j < fadded.size(); j++){ - printf("start adding mask %i\n",i); - cv::Mat mask = masks[i][fadded[j]]; - - cv_bridge::CvImage maskBridgeImage; - maskBridgeImage.image = mask; - maskBridgeImage.encoding = "mono8"; - - quasimodo_msgs::model_from_frame mff; - mff.request.mask = *(maskBridgeImage.toImageMsg()); - mff.request.isnewmodel = (j == (fadded.size()-1)); - mff.request.frame_id = fid[j]; - - if (model_from_frame_client.call(mff)){//Build model from frame - int model_id = mff.response.model_id; - if(model_id > 0){ - ROS_INFO("model_id%i", model_id ); - } - }else{ROS_ERROR("Failed to call service index_frame");} - - printf("stop adding mask %i\n",i); - - } - } + geometry_msgs::Pose pose; +// pose.orientation = tfmsg.rotation; +// pose.position.x = tfmsg.translation.x; +// pose.position.y = tfmsg.translation.y; +// pose.position.z = tfmsg.translation.z; + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + pose.orientation.w = q.w(); + pose.position.x = m(0,3); + pose.position.y = m(1,3); + pose.position.z = m(2,3); + + + cv_bridge::CvImage rgbBridgeImage; + rgbBridgeImage.image = rgbimage; + rgbBridgeImage.encoding = "bgr8"; + + cv_bridge::CvImage depthBridgeImage; + depthBridgeImage.image = depthimage; + depthBridgeImage.encoding = "mono16"; + + quasimodo_msgs::index_frame ifsrv; + ifsrv.request.frame.capture_time = ros::Time(); + ifsrv.request.frame.pose = pose; + ifsrv.request.frame.frame_id = -1; + ifsrv.request.frame.rgb = *(rgbBridgeImage.toImageMsg()); + ifsrv.request.frame.depth = *(depthBridgeImage.toImageMsg()); + + + //314.458000 242.038000 536.458000 537.422000 + ifsrv.request.frame.camera.K[0] = 536.458000;//cams[i][j].fx(); + ifsrv.request.frame.camera.K[4] = 537.422000;//cams[i][j].fy(); + ifsrv.request.frame.camera.K[2] = 314.458000;//cams[i][j].cx(); + ifsrv.request.frame.camera.K[5] = 242.038000;//cams[i][j].cy(); + + printf("camera: %f %f %f %f\n",cams[i][j].cx(),cams[i][j].cy(),cams[i][j].fx(),cams[i][j].fy()); + + if (index_frame_client.call(ifsrv)){//Add frame to model server + int frame_id = ifsrv.response.frame_id; + fadded.push_back(j); + fid.push_back(frame_id); + ROS_INFO("frame_id%i", frame_id ); + }else{ROS_ERROR("Failed to call service index_frame");} + + printf("stop adding frame %i\n",i); + } + + for(unsigned int j = 0; j < fadded.size(); j++){ + printf("start adding mask %i\n",i); + cv::Mat mask = masks[i][fadded[j]]; + + cv_bridge::CvImage maskBridgeImage; + maskBridgeImage.image = mask; + maskBridgeImage.encoding = "mono8"; + + quasimodo_msgs::model_from_frame mff; + mff.request.mask = *(maskBridgeImage.toImageMsg()); + mff.request.isnewmodel = (j == (fadded.size()-1)); + mff.request.frame_id = fid[j]; + + if (model_from_frame_client.call(mff)){//Build model from frame + int model_id = mff.response.model_id; + if(model_id > 0){ + ROS_INFO("model_id%i", model_id ); + } + }else{ROS_ERROR("Failed to call service index_frame");} + + printf("stop adding mask %i\n",i); + + } + } +} + +pcl::PointCloud::Ptr transformCloud(pcl::PointCloud::Ptr cloud, Eigen::Matrix4d p){ + pcl::PointCloud::Ptr ret (new pcl::PointCloud); + ret->points.resize(cloud->points.size()); + + const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3); + const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3); + const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3); + + for(unsigned int i = 0; i < cloud->points.size(); i++){ + pcl::PointXYZRGBNormal & p1 = cloud->points[i]; + pcl::PointXYZRGBNormal & p2 = ret->points[i]; + + p2.r = p1.r; + p2.g = p1.g; + p2.b = p1.b; + + const double & src_x = p1.x; + const double & src_y = p1.y; + const double & src_z = p1.z; + + const double & src_nx = p1.normal_x; + const double & src_ny = p1.normal_y; + const double & src_nz = p1.normal_z; + + p2.x = m00*src_x + m01*src_y + m02*src_z + m03; + p2.y = m10*src_x + m11*src_y + m12*src_z + m13; + p2.z = m20*src_x + m21*src_y + m22*src_z + m23; + + p2.normal_x = m00*src_nx + m01*src_ny + m02*src_nz; + p2.normal_y = m10*src_nx + m11*src_ny + m12*src_nz; + p2.normal_z = m20*src_nx + m21*src_ny + m22*src_nz; + + p2.r = p1.r; + p2.g = p1.g; + p2.b = p1.b; + } + + return ret; +} + +pcl::PointCloud::Ptr getNC(pcl::PointCloud::Ptr cloud, int nr_data = 5000000){ + int MaxDepthChangeFactor = 20; + int NormalSmoothingSize = 7; + int depth_dependent_smoothing = 1; + + pcl::IntegralImageNormalEstimation ne; + ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); + ne.setNormalSmoothingSize(NormalSmoothingSize); + ne.setDepthDependentSmoothing (depth_dependent_smoothing); + ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); + + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + ne.setInputCloud(cloud); + ne.compute(*normals); + + int rc = rand()%256; + int gc = rand()%256; + int bc = rand()%256; + + pcl::PointCloud::Ptr ret (new pcl::PointCloud); + pcl::concatenateFields(*cloud,*normals,*ret); + + boost::shared_ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "sample cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); + viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (ret, pcl::visualization::PointCloudColorHandlerRGBField(ret), "sample cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); + viewer->addPointCloudNormals (ret, ret, 15, 0.10, "normals"); + viewer->spin(); + + +// ret->points.resize(cloud->points.size()); +// for(int i = 0; i < cloud->points.size(); i++){ +// pcl::PointXYZRGBNormal & p1 = ret->points[i]; +// pcl::PointXYZRGB & p = cloud->points[i]; +// pcl::Normal & pn = normals->points[i]; + +// p1.x = p.x; +// p1.y = p.y; +// p1.z = p.z; +//// p1.r = rc; +//// p1.g = gc; +//// p1.b = bc; +// p1.r = p.r; +// p1.g = p.g; +// p1.b = p.b; +// p1.normal_x = pn.normal_x; +// p1.normal_y = pn.normal_y; +// p1.normal_z = pn.normal_z; +// } +/* + +*/ + +// for(int i = 0; i < ret->points.size(); i++){ +// pcl::PointXYZRGBNormal & p1 = ret->points[i]; +// p1.r = rc; +// p1.g = gc; +// p1.b = bc; +// } + return ret; +} + +int nr_vAdditionalViews = 4; + +void load(std::string sweep_xml){ + + int slash_pos = sweep_xml.find_last_of("/"); + std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; + printf("folder: %s\n",sweep_folder.c_str()); + + SimpleXMLParser parser; + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); + vector objects = semantic_map_load_utilties::loadAllDynamicObjectsFromSingleSweep(sweep_folder+"room.xml"); + + int object_id; + for (unsigned int i=0; i< objects.size(); i++){ + if (objects[i].objectScanIndices.size() != 0){object_id = i;} + } + int index = objectFiles[object_id].toStdString().find_last_of("."); + string object_root_name = objectFiles[object_id].toStdString().substr(0,index); + + for (auto object : objects){ + if (!object.objectScanIndices.size()){continue;} + + nr_vAdditionalViews = 1 + rand()%5; + + std::vector viewrgbs; + std::vector viewdepths; + std::vector viewmasks; + std::vector viewtfs; + std::vector viewposes; + std::vector< image_geometry::PinholeCameraModel > viewcams; + char buf [1024]; + sprintf(buf,"%s/object_%i/poses.txt",sweep_folder.c_str(),object_id); + std::vector poses = getRegisteredViewPoses(std::string(buf), object.vAdditionalViews.size()); + cout<<"Loaded "<height,cloud->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)mask.data; + + cv::Mat rgb; + rgb.create(cloud->height,cloud->width,CV_8UC3); + unsigned char * rgbdata = (unsigned char *)rgb.data; + + cv::Mat depth; + depth.create(cloud->height,cloud->width,CV_16UC1); + unsigned short * depthdata = (unsigned short *)depth.data; + + unsigned int nr_data = cloud->height * cloud->width; + for(unsigned int j = 0; j < nr_data; j++){ + maskdata[j] = 0; + PointType p = cloud->points[j]; + rgbdata[3*j+0] = p.b; + rgbdata[3*j+1] = p.g; + rgbdata[3*j+2] = p.r; + depthdata[j] = short(5000.0 * p.z); + } + + cout<<"Processing AV "<>av_index; + av_mask->push_back(object.vAdditionalViews[i]->points[av_index]); + maskdata[av_index] = 255; + } + + viewrgbs.push_back(rgb); + viewdepths.push_back(depth); + viewmasks.push_back(mask); + viewtfs.push_back(object.vAdditionalViewsTransforms[i]); + viewposes.push_back(poses[i+1]); + viewcams.push_back(roomData.vIntermediateRoomCloudCamParams.front()); + + cv::namedWindow("rgbimage", cv::WINDOW_AUTOSIZE); + cv::imshow( "rgbimage", rgb); + cv::namedWindow("depthimage", cv::WINDOW_AUTOSIZE); + cv::imshow( "depthimage", depth); + cv::namedWindow("mask", cv::WINDOW_AUTOSIZE); + cv::imshow( "mask", mask); + cv::waitKey(30); + } + + if(viewrgbs.size() > 0){ + + rgbs.push_back(viewrgbs); + depths.push_back(viewdepths); + masks.push_back(viewmasks); + tfs.push_back(viewtfs); + initposes.push_back(viewposes); + cams.push_back(viewcams); +/* + tf::StampedTransform tf = roomData.vIntermediateRoomCloudTransforms.front(); + geometry_msgs::TransformStamped tfstmsg; + tf::transformStampedTFToMsg (tf, tfstmsg); + geometry_msgs::Transform tfmsg = tfstmsg.transform; + geometry_msgs::Pose pose; + pose.orientation = tfmsg.rotation; + pose.position.x = tfmsg.translation.x; + pose.position.y = tfmsg.translation.y; + pose.position.z = tfmsg.translation.z; + Eigen::Affine3d localthing; + tf::poseMsgToEigen(pose, localthing); + + reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS + cam->fx = viewcams.front().fx(); + cam->fy = viewcams.front().fy(); + cam->cx = viewcams.front().cx(); + cam->cy = viewcams.front().cy(); + + + cv::Mat fullmask; + fullmask.create(480,640,CV_8UC1); + unsigned char * maskdata = (unsigned char *)fullmask.data; + for(int j = 0; j < 480*640; j++){maskdata[j] = 0;} + + + reglib::Model * objectModel = 0;//new reglib::Model(frames[frame_id],mask); + + std::vector current_frames; + for (unsigned int i=0;i < nr_vAdditionalViews && iframes.push_back(frame); + objectModel->relativeposes.push_back(current_frames.front()->pose.inverse() * frame->pose); + objectModel->modelmasks.push_back(new reglib::ModelMask(viewmasks[i]));//fullmask)); + } + } + + reglib::Model * sweepmodel = 0; + std::vector current_room_frames; + for (size_t i=0; ipoints.size()<frames.push_back(frame); + sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose); + sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); + } + } + sweepmodel->recomputeModelPoints(); + +// objectModel->submodels.push_back(sweepmodel); +// objectModel->submodels_relativeposes.push_back(objectModel->frames.front()->pose.inverse() * sweepmodel->frames.front()->pose); + + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( objectModel, reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + objectModel->print(); + mu->makeInitialSetup(); + objectModel->print(); + delete mu; + + objectModel->recomputeModelPoints(); + + reglib::Model * newmodelHolder = new reglib::Model(); + newmodelHolder->submodels.push_back(objectModel); + newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + newmodelHolder->recomputeModelPoints(); + + pcl::PointCloud::Ptr cn1 = objectModel->getPCLnormalcloud(1, false); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cn1, pcl::visualization::PointCloudColorHandlerRGBField(cn1), "sample cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); + viewer->spin(); + + pcl::PointCloud::Ptr cn = sweepmodel->getPCLnormalcloud(1, false); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cn, pcl::visualization::PointCloudColorHandlerRGBField(cn), "sample cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); + viewer->spin(); +*/ +// rgbs.push_back(viewrgbs); +// depths.push_back(viewdepths); +// masks.push_back(viewmasks); +// tfs.push_back(viewtfs); +// initposes.push_back(viewposes); +// cams.push_back(viewcams); + } + } } int main(int argc, char** argv){ - ros::init(argc, argv, "use_rares_client"); - ros::NodeHandle n; - ros::Subscriber sub = n.subscribe("modelserver", 1000, chatterCallback); + ros::init(argc, argv, "use_rares_client"); + ros::NodeHandle n; + ros::Subscriber sub = n.subscribe("modelserver", 1000, chatterCallback); - pg = new pcl::visualization::PCLVisualizer (argc, argv, "global_transform"); - pg->addCoordinateSystem(); + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); - ros::NodeHandle pn("~"); + ros::NodeHandle pn("~"); for(int ar = 1; ar < argc; ar++){ string overall_folder = std::string(argv[ar]); - //string overall_folder = "/media/nbore/My Passport/icra_data_surfels/controlled_experiments/object_2_fire_extinguisher"; - //overall_folder = "/media/johane/SSDstorage/icra_data_surfels/controlled_experiments/object_7_microwave/"; - //overall_folder = "/media/johane/SSDstorage/icra_data_surfels/controlled_experiments/object_6_router_box/"; - //overall_folder = "/media/johane/SSDstorage/icra_data_surfels/controlled_experiments/"; - //pn.param("folder", overall_folder, ""); - - vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); for (auto sweep_xml : sweep_xmls) { - //if(viewrgbs.size() > 0){break;} - //if(rgbs.size() > 0){break;} - int slash_pos = sweep_xml.find_last_of("/"); - std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; - printf("folder: %s\n",sweep_folder.c_str()); - - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); - vector objects = semantic_map_load_utilties::loadAllDynamicObjectsFromSingleSweep(sweep_folder+"room.xml"); - - int object_id; - for (unsigned int i=0; i< objects.size(); i++){ - if (objects[i].objectScanIndices.size() != 0){object_id = i;} - } - int index = objectFiles[object_id].toStdString().find_last_of("."); - string object_root_name = objectFiles[object_id].toStdString().substr(0,index); - - for (auto object : objects){ - if (!object.objectScanIndices.size()){continue;} - - //if(rgbs.size() > 0){break;} - pg->removeAllPointClouds(); - printf("%i\n",int(object.vAdditionalViews.size())); - - std::vector viewrgbs; - std::vector viewdepths; - std::vector viewmasks; - std::vector viewtfs; - std::vector viewposes; - //std::stringstream ss_pose_file; ss_pose_file< poses = getRegisteredViewPoses(std::string(buf), object.vAdditionalViews.size()); - cout<<"Loaded "< rp = getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms) - // view AV clouds and masks - - int step = std::max(1,int(0.5+double(object.vAdditionalViews.size())/20.0)); - for (unsigned int i=0; i < 2000 && iheight,cloud->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)mask.data; - - cv::Mat rgb; - rgb.create(cloud->height,cloud->width,CV_8UC3); - unsigned char * rgbdata = (unsigned char *)rgb.data; - - cv::Mat depth; - depth.create(cloud->height,cloud->width,CV_16UC1); - unsigned short * depthdata = (unsigned short *)depth.data; - - unsigned int nr_data = cloud->height * cloud->width; - for(unsigned int j = 0; j < nr_data; j++){ - maskdata[j] = 0; - PointType p = cloud->points[j]; - rgbdata[3*j+0] = p.b; - rgbdata[3*j+1] = p.g; - rgbdata[3*j+2] = p.r; - depthdata[j] = short(5000.0 * p.z); - } - - cout<<"Processing AV "<>av_index; - av_mask->push_back(object.vAdditionalViews[i]->points[av_index]); - maskdata[av_index] = 255; - } - - viewrgbs.push_back(rgb); - viewdepths.push_back(depth); - viewmasks.push_back(mask); - viewtfs.push_back(object.vAdditionalViewsTransforms[i]); - viewposes.push_back(poses[i+1]); - - cv::namedWindow("rgbimage", cv::WINDOW_AUTOSIZE); - cv::imshow( "rgbimage", rgb); - cv::namedWindow("depthimage", cv::WINDOW_AUTOSIZE); - cv::imshow( "depthimage", depth); - cv::namedWindow("mask", cv::WINDOW_AUTOSIZE); - cv::imshow( "mask", mask); - cv::waitKey(30); - // { - // std::cout << poses[i+1] << std::endl << std::endl; - // pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); - // pcl::transformPointCloud (*(av_mask), *transformed_cloud, poses[i+1]); - // PointCloudColorHandlerCustom intermediate_h (transformed_cloud, 255, 0, 0); - // pg->addPointCloud(transformed_cloud, intermediate_h, (complete_av_mask_name+"av").c_str()); - // pg->spin(); - // } - } - - if(viewrgbs.size() > 0){ - //pg->spin(); - rgbs.push_back(viewrgbs); - depths.push_back(viewdepths); - masks.push_back(viewmasks); - tfs.push_back(viewtfs); - initposes.push_back(viewposes); - } - } + load(sweep_xml); } } - for(unsigned int i = 0; i < rgbs.size(); i++){ - int ind = rand()%rgbs.size(); - std::iter_swap(rgbs.begin()+i,rgbs.begin()+ind); - std::iter_swap(depths.begin()+i,depths.begin()+ind); - std::iter_swap(masks.begin()+i,masks.begin()+ind); - std::iter_swap(tfs.begin()+i,tfs.begin()+ind); - std::iter_swap(initposes.begin()+i,initposes.begin()+ind); - } + for(unsigned int i = 0; i < rgbs.size(); i++){ + int ind = rand()%rgbs.size(); + std::iter_swap(rgbs.begin()+i,rgbs.begin()+ind); + std::iter_swap(depths.begin()+i,depths.begin()+ind); + std::iter_swap(masks.begin()+i,masks.begin()+ind); + std::iter_swap(tfs.begin()+i,tfs.begin()+ind); + std::iter_swap(initposes.begin()+i,initposes.begin()+ind); + } - model_from_frame_client = n.serviceClient("model_from_frame"); - fuse_models_client = n.serviceClient( "fuse_models"); - get_model_client = n.serviceClient( "get_model"); - index_frame_client = n.serviceClient( "index_frame"); + model_from_frame_client = n.serviceClient("model_from_frame"); + fuse_models_client = n.serviceClient( "fuse_models"); + get_model_client = n.serviceClient( "get_model"); + index_frame_client = n.serviceClient( "index_frame"); - ros::spin(); + ros::spin(); } diff --git a/quasimodo/quasimodo_brain/src/robot_listener.cpp b/quasimodo/quasimodo_brain/src/robot_listener.cpp index ebac8c53..0895a8b7 100644 --- a/quasimodo/quasimodo_brain/src/robot_listener.cpp +++ b/quasimodo/quasimodo_brain/src/robot_listener.cpp @@ -228,6 +228,8 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg) // pose.position.z = m(2,3); + + cv_bridge::CvImage rgbBridgeImage; rgbBridgeImage.image = rgbimage; rgbBridgeImage.encoding = "bgr8"; @@ -243,6 +245,10 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg) ifsrv.request.frame.rgb = *(rgbBridgeImage.toImageMsg()); ifsrv.request.frame.depth = *(depthBridgeImage.toImageMsg()); + ifsrv.request.frame.camera.K[0] = 536.458000;//cams[i][j].fx(); + ifsrv.request.frame.camera.K[4] = 537.422000;//cams[i][j].fy(); + ifsrv.request.frame.camera.K[2] = 314.458000;//cams[i][j].cx(); + ifsrv.request.frame.camera.K[5] = 242.038000;//cams[i][j].cy(); if (index_frame_client.call(ifsrv)){//Add frame to model server int frame_id = ifsrv.response.frame_id; fadded.push_back(j); diff --git a/quasimodo/quasimodo_brain/src/test_align2.cpp b/quasimodo/quasimodo_brain/src/test_align2.cpp new file mode 100644 index 00000000..c17839eb --- /dev/null +++ b/quasimodo/quasimodo_brain/src/test_align2.cpp @@ -0,0 +1,303 @@ +#include +#include +#include + +#include + +#include "../../quasimodo_models/include/modelupdater/ModelUpdater.h" +#include "core/RGBDFrame.h" +#include "core/Util.h" +#include + +#include + +#include "ceres/ceres.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "ceres/rotation.h" +#include "ceres/iteration_callback.h" + +using namespace std; +using namespace Eigen; +using namespace reglib; + +using ceres::NumericDiffCostFunction; +using ceres::SizedCostFunction; +using ceres::CENTRAL; +using ceres::AutoDiffCostFunction; +using ceres::CostFunction; +using ceres::Problem; +using ceres::Solver; +using ceres::Solve; +using ceres::Solve; + +default_random_engine generator; +Matrix3Xd conc(Matrix3Xd A, Matrix3Xd B){ + Eigen::Matrix3Xd C = Eigen::Matrix3Xd::Zero(3, A.cols()+B.cols()); + C << A, B; + return C; +} +Matrix3Xd getPoints(int cols){ + Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3,cols); + for(int i = 0; i < 3; i++){ + for(int j = 0; j < cols; j++){ + points(i,j) = (rand()%10000)*0.0001; + } + } + return points; +} +Matrix3Xd getMeasurements(Matrix3Xd gt, double noise){ + int cols = gt.cols(); + + normal_distribution distribution(0,noise); + Eigen::Matrix3Xd points = Eigen::Matrix3Xd::Zero(3,cols); + for(int i = 0; i < 3; i++){ + for(int j = 0; j < cols; j++){ + points(i,j) = gt(i,j) + distribution(generator); + } + } + return points; +} + +Matrix3Xd transform_points(Matrix3Xd points, Matrix4d transform){ + int cols = points.cols(); + + float m00 = transform(0,0); float m01 = transform(0,1); float m02 = transform(0,2); float m03 = transform(0,3); + float m10 = transform(1,0); float m11 = transform(1,1); float m12 = transform(1,2); float m13 = transform(1,3); + float m20 = transform(2,0); float m21 = transform(2,1); float m22 = transform(2,2); float m23 = transform(2,3); + + Eigen::Matrix3Xd points2 = Eigen::Matrix3Xd::Zero(3,cols); + for(int i = 0; i < cols; i++){ + float x = points(0,i); + float y = points(1,i); + float z = points(2,i); + points2(0,i) = m00*x + m01*y + m02*z + m03; + points2(1,i) = m10*x + m11*y + m12*z + m13; + points2(2,i) = m20*x + m21*y + m22*z + m23; + } + return points2; +} + +double get_reconst_rms(Matrix3Xd gt, Matrix3Xd points, int nr_points){ + double rms = 0; + for(int i = 0; i < nr_points; i++){ + float dx = gt(0,i) - points(0,i); + float dy = gt(1,i) - points(1,i); + float dz = gt(2,i) - points(2,i); + rms += dx*dx+dy*dy+dz*dz; + } + rms/= double(nr_points); + rms = sqrt(rms); + return rms; +} + + +#include + +Matrix4d align1(Matrix3Xd gt, Matrix3Xd points){ + pcl::TransformationFromCorrespondences tfc; + for(int i = 0; i < gt.cols(); i++){ + tfc.add(Eigen::Vector3f(gt(0,i),gt(1,i),gt(2,i)),Eigen::Vector3f(points(0,i),points(1,i),points(2,i))); + } + return tfc.getTransformation().matrix().cast().inverse(); +} + + +Eigen::Matrix4d constructTransformationMatrixtest (const double & alpha, const double & beta, const double & gamma, const double & tx, const double & ty, const double & tz){ + // Construct the transformation matrix from rotation and translation + Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Zero (); + transformation_matrix (0, 0) = cos (gamma) * cos (beta); + transformation_matrix (0, 1) = -sin (gamma) * cos (alpha) + cos (gamma) * sin (beta) * sin (alpha); + transformation_matrix (0, 2) = sin (gamma) * sin (alpha) + cos (gamma) * sin (beta) * cos (alpha); + transformation_matrix (1, 0) = sin (gamma) * cos (beta); + transformation_matrix (1, 1) = cos (gamma) * cos (alpha) + sin (gamma) * sin (beta) * sin (alpha); + transformation_matrix (1, 2) = -cos (gamma) * sin (alpha) + sin (gamma) * sin (beta) * cos (alpha); + transformation_matrix (2, 0) = -sin (beta); + transformation_matrix (2, 1) = cos (beta) * sin (alpha); + transformation_matrix (2, 2) = cos (beta) * cos (alpha); + + transformation_matrix (0, 3) = tx; + transformation_matrix (1, 3) = ty; + transformation_matrix (2, 3) = tz; + transformation_matrix (3, 3) = 1; + return transformation_matrix; +} + + +Matrix4d align2(Matrix3Xd gt, Matrix3Xd points){ + Affine3d p = Affine3d::Identity(); + for(int l = 0; l < 25; l++){ + + + typedef Eigen::Matrix Vector6d; + typedef Eigen::Matrix Matrix6d; + + Matrix6d ATA; + Vector6d ATb; + ATA.setZero (); + ATb.setZero (); + + const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3); + const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3); + const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3); + + double scoreX = 0; + double scoreY = 0; + double scoreZ = 0; + double scoreALL = 0; + + double wsum = 0; + double wsx = 0; + double wsy = 0; + double wsz = 0; + for(int i = 0; i < gt.cols(); i++){ + const double & src_x = gt(0,i); + const double & src_y = gt(1,i); + const double & src_z = gt(2,i); + + const double & dx = points(0,i); + const double & dy = points(1,i); + const double & dz = points(2,i); + + const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; + const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; + const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; + + const double diffX = dx-sx; + const double diffY = dy-sy; + const double diffZ = dz-sz; + + scoreX += diffX*diffX; + scoreY += diffY*diffY; + scoreZ += diffZ*diffZ; + scoreALL += diffX*diffX+diffY*diffY+diffZ*diffZ; + + double weight = 1; + wsum += weight; + + wsx += weight * sx; + wsy += weight * sy; + wsz += weight * sz; + + double wsxsx = weight * sx*sx; + double wsysy = weight * sy*sy; + double wszsz = weight * sz*sz; + + ATA.coeffRef (0) += wsysy + wszsz;//a0 * a0; + ATA.coeffRef (1) -= weight * sx*sy;//a0 * a1; + ATA.coeffRef (2) -= weight * sz*sx;//a0 * a2; + + + ATA.coeffRef (7) += wsxsx + wszsz;//a1 * a1; + ATA.coeffRef (8) -= weight * sy*sz;//a1 * a2; + + ATA.coeffRef (14) += wsxsx + wsysy;//a2 * a2; + + ATb.coeffRef (0) += weight * (sy*diffZ -sz*diffY);//a0 * b; + ATb.coeffRef (1) += weight * (-sx*diffZ + sz*diffX);//a1 * b; + ATb.coeffRef (2) += weight * (sx*diffY -sy*diffX);//a2 * b; + ATb.coeffRef (3) += weight * diffX;//a3 * b; + ATb.coeffRef (4) += weight * diffY;//a4 * b; + ATb.coeffRef (5) += weight * diffZ;//a5 * b; + } + + + ATA.coeffRef (4) -= wsz;//a0 * a4; + ATA.coeffRef (9) += wsz;//a1 * a3; + + ATA.coeffRef (5) += wsy;//a0 * a5; + ATA.coeffRef (15) -= wsy;//a2 * a3; + + ATA.coeffRef (11) -= wsx;//a1 * a5; + ATA.coeffRef (16) += wsx;//a2 * a4; + + ATA.coeffRef (21) += wsum;//a3 * a3; + + ATA.coeffRef (28) += wsum;//a4 * a4; + + ATA.coeffRef (35) += wsum;//a5 * a5; + + ATA.coeffRef (6) = ATA.coeff (1); + ATA.coeffRef (12) = ATA.coeff (2); + ATA.coeffRef (13) = ATA.coeff (8); + ATA.coeffRef (18) = ATA.coeff (3); + ATA.coeffRef (19) = ATA.coeff (9); + ATA.coeffRef (20) = ATA.coeff (15); + ATA.coeffRef (24) = ATA.coeff (4); + ATA.coeffRef (25) = ATA.coeff (10); + ATA.coeffRef (26) = ATA.coeff (16); + ATA.coeffRef (27) = ATA.coeff (22); + ATA.coeffRef (30) = ATA.coeff (5); + ATA.coeffRef (31) = ATA.coeff (11); + ATA.coeffRef (32) = ATA.coeff (17); + ATA.coeffRef (33) = ATA.coeff (23); + ATA.coeffRef (34) = ATA.coeff (29); + + ATA(0,0) += wsum*0.000001; + ATA(1,1) += wsum*0.000001; + ATA(2,2) += wsum*0.000001; + ATA(3,3) += wsum*0.000001; + ATA(4,4) += wsum*0.000001; + ATA(5,5) += wsum*0.000001; + + //printf("--------------------\n"); + printf("scoreX: %5.5f ",scoreX); + printf("scoreY: %5.5f ",scoreY); + printf("scoreZ: %5.5f ",scoreZ); + printf("scoreALL: %5.5f\n",scoreALL); + + // Solve A*x = b + Vector6d x = static_cast (ATA.inverse () * ATb); + + Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrixtest(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0))); + // std::cout << ATA << std::endl << std::endl; + // std::cout << ATb.transpose() << std::endl << std::endl; + // std::cout << ATA.inverse () << std::endl << std::endl; + // std::cout << x.transpose() << std::endl << std::endl; + // std::cout << transformation.matrix() << std::endl << std::endl; + p = transformation*p; + + } + return p.matrix().inverse(); +} + +int main(int argc, char **argv){ + int nr_tests = 1; + int nr_frames = 100; + int points_per_match = 100; + double overlap_prob = 0.1; + int nr_points = 5000; + double noise = 0.01; + + for(int nt = 0; nt < nr_tests; nt++){ + Matrix3Xd gt = getPoints(nr_points); + Matrix3Xd measurements = getMeasurements(gt,noise); + + double angle1 = double(0.1);double angle2 = 0.1;double angle3 = 0.1; + double t1 = double(0.1);double t2 = 0; double t3 = 0; + + //printf("\%\% transformation %i -> angle: %f %f %f translation: %f %f %f\n",i,angle1,angle2,angle3,t1,t2,t3); + Matrix3d rotmat; + rotmat = AngleAxisd(angle1, Vector3d::UnitZ()) * AngleAxisd(angle2, Vector3d::UnitY()) * AngleAxisd(angle3, Vector3d::UnitZ()); + Matrix4d transformation = Matrix4d::Identity(); + transformation.block(0,0,3,3) = rotmat; + transformation(0,3) = t1; transformation(1,3) = t2; transformation(2,3) = t3; + + Matrix3Xd measurements_trans = transform_points(measurements, transformation); + + Matrix4d transformation_est1 = align1(gt,measurements_trans); + Matrix3Xd measurements_trans_est1 = transform_points(measurements_trans, transformation_est1); + + Matrix4d transformation_est2 = align2(gt,measurements_trans); + Matrix3Xd measurements_trans_est2 = transform_points(measurements_trans, transformation_est2); + + double opt_rms = get_reconst_rms(gt, measurements, nr_points)/sqrt(3.0); + double rms1 = get_reconst_rms(gt, measurements_trans_est1, nr_points)/sqrt(3.0); + double rms2 = get_reconst_rms(gt, measurements_trans_est2, nr_points)/sqrt(3.0); + printf("rms1: %12.12f\n",10000.0*(rms1-opt_rms)); + printf("rms2: %12.12f\n",10000.0*(rms2-opt_rms)); + + + } + return 0; +} diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp new file mode 100644 index 00000000..76d0dcef --- /dev/null +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -0,0 +1,663 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include +#include + +#include +#include + +#include "eigen_conversions/eigen_msg.h" +#include "tf_conversions/tf_eigen.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" +#include "metaroom_xml_parser/simple_summary_parser.h" + +#include + +#include "ros/ros.h" +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" + +#include "ros/ros.h" +#include +#include +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" + +#include +#include + +#include "modelupdater/ModelUpdater.h" +#include "core/RGBDFrame.h" + + +using namespace std; + +typedef pcl::PointXYZRGB PointType; +typedef pcl::PointCloud Cloud; +typedef typename Cloud::Ptr CloudPtr; +typedef pcl::search::KdTree Tree; +typedef semantic_map_load_utilties::DynamicObjectData ObjectData; + +using pcl::visualization::PointCloudColorHandlerCustom; + +std::vector< std::vector > rgbs; +std::vector< std::vector > depths; +std::vector< std::vector > masks; +std::vector< std::vector >tfs; +std::vector< std::vector > initposes; +std::vector< std::vector< image_geometry::PinholeCameraModel > > cams; + +pcl::visualization::PCLVisualizer* pg; +boost::shared_ptr viewer; + + + +std::vector getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms){ + std::vector toRet; + ifstream in(poses_file); + if (!in.is_open()){ + cout<<"ERROR: cannot find poses file "<> temp; + transform(j,k) = temp; + } + } + toRet.push_back(transform); + } + return toRet; +} + +void chatterCallback(const std_msgs::String::ConstPtr& msg) +{ + ROS_INFO("I heard: [%s]", msg->data.c_str()); + + for(unsigned int i = 1; i < rgbs.size(); i++){ + std::vector fid; + std::vector fadded; + for(unsigned int j = 0; j < rgbs[i].size(); j++){ + printf("%i %i\n",i,j); + + //cv::Mat maskimage = masks[i][j]; + cv::Mat rgbimage = rgbs[i][j]; + cv::Mat depthimage = depths[i][j]; + tf::StampedTransform tf = tfs[i][j]; + Eigen::Matrix4f m = initposes[i][j]; + geometry_msgs::TransformStamped tfstmsg; + tf::transformStampedTFToMsg (tf, tfstmsg); + + geometry_msgs::Transform tfmsg = tfstmsg.transform; + + printf("start adding frame %i\n",i); + + Eigen::Quaternionf q = Eigen::Quaternionf(Eigen::Affine3f(m).rotation()); + + geometry_msgs::Pose pose; +// pose.orientation = tfmsg.rotation; +// pose.position.x = tfmsg.translation.x; +// pose.position.y = tfmsg.translation.y; +// pose.position.z = tfmsg.translation.z; + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + pose.orientation.w = q.w(); + pose.position.x = m(0,3); + pose.position.y = m(1,3); + pose.position.z = m(2,3); + + + cv_bridge::CvImage rgbBridgeImage; + rgbBridgeImage.image = rgbimage; + rgbBridgeImage.encoding = "bgr8"; + + cv_bridge::CvImage depthBridgeImage; + depthBridgeImage.image = depthimage; + depthBridgeImage.encoding = "mono16"; +/* + quasimodo_msgs::index_frame ifsrv; + ifsrv.request.frame.capture_time = ros::Time(); + ifsrv.request.frame.pose = pose; + ifsrv.request.frame.frame_id = -1; + ifsrv.request.frame.rgb = *(rgbBridgeImage.toImageMsg()); + ifsrv.request.frame.depth = *(depthBridgeImage.toImageMsg()); + + + //314.458000 242.038000 536.458000 537.422000 + ifsrv.request.frame.camera.K[0] = 536.458000;//cams[i][j].fx(); + ifsrv.request.frame.camera.K[4] = 537.422000;//cams[i][j].fy(); + ifsrv.request.frame.camera.K[2] = 314.458000;//cams[i][j].cx(); + ifsrv.request.frame.camera.K[5] = 242.038000;//cams[i][j].cy(); + + printf("camera: %f %f %f %f\n",cams[i][j].cx(),cams[i][j].cy(),cams[i][j].fx(),cams[i][j].fy()); + + if (index_frame_client.call(ifsrv)){//Add frame to model server + int frame_id = ifsrv.response.frame_id; + fadded.push_back(j); + fid.push_back(frame_id); + ROS_INFO("frame_id%i", frame_id ); + }else{ROS_ERROR("Failed to call service index_frame");} +*/ + printf("stop adding frame %i\n",i); + } + + for(unsigned int j = 0; j < fadded.size(); j++){ + printf("start adding mask %i\n",i); + cv::Mat mask = masks[i][fadded[j]]; + + cv_bridge::CvImage maskBridgeImage; + maskBridgeImage.image = mask; + maskBridgeImage.encoding = "mono8"; +/* + quasimodo_msgs::model_from_frame mff; + mff.request.mask = *(maskBridgeImage.toImageMsg()); + mff.request.isnewmodel = (j == (fadded.size()-1)); + mff.request.frame_id = fid[j]; + + if (model_from_frame_client.call(mff)){//Build model from frame + int model_id = mff.response.model_id; + if(model_id > 0){ + ROS_INFO("model_id%i", model_id ); + } + }else{ROS_ERROR("Failed to call service index_frame");} +*/ + printf("stop adding mask %i\n",i); + + } + } +} + +pcl::PointCloud::Ptr transformCloud(pcl::PointCloud::Ptr cloud, Eigen::Matrix4d p){ + pcl::PointCloud::Ptr ret (new pcl::PointCloud); + ret->points.resize(cloud->points.size()); + + const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3); + const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3); + const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3); + + for(unsigned int i = 0; i < cloud->points.size(); i++){ + pcl::PointXYZRGBNormal & p1 = cloud->points[i]; + pcl::PointXYZRGBNormal & p2 = ret->points[i]; + + p2.r = p1.r; + p2.g = p1.g; + p2.b = p1.b; + + const double & src_x = p1.x; + const double & src_y = p1.y; + const double & src_z = p1.z; + + const double & src_nx = p1.normal_x; + const double & src_ny = p1.normal_y; + const double & src_nz = p1.normal_z; + + p2.x = m00*src_x + m01*src_y + m02*src_z + m03; + p2.y = m10*src_x + m11*src_y + m12*src_z + m13; + p2.z = m20*src_x + m21*src_y + m22*src_z + m23; + + p2.normal_x = m00*src_nx + m01*src_ny + m02*src_nz; + p2.normal_y = m10*src_nx + m11*src_ny + m12*src_nz; + p2.normal_z = m20*src_nx + m21*src_ny + m22*src_nz; + + p2.r = p1.r; + p2.g = p1.g; + p2.b = p1.b; + } + + return ret; +} + +pcl::PointCloud::Ptr getNC(pcl::PointCloud::Ptr cloud, int nr_data = 5000000){ + int MaxDepthChangeFactor = 20; + int NormalSmoothingSize = 7; + int depth_dependent_smoothing = 1; + + pcl::IntegralImageNormalEstimation ne; + ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); + ne.setNormalSmoothingSize(NormalSmoothingSize); + ne.setDepthDependentSmoothing (depth_dependent_smoothing); + ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); + + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + ne.setInputCloud(cloud); + ne.compute(*normals); + + int rc = rand()%256; + int gc = rand()%256; + int bc = rand()%256; + + pcl::PointCloud::Ptr ret (new pcl::PointCloud); + pcl::concatenateFields(*cloud,*normals,*ret); + + boost::shared_ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "sample cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); + viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (ret, pcl::visualization::PointCloudColorHandlerRGBField(ret), "sample cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); + viewer->addPointCloudNormals (ret, ret, 15, 0.10, "normals"); + viewer->spin(); + + +// ret->points.resize(cloud->points.size()); +// for(int i = 0; i < cloud->points.size(); i++){ +// pcl::PointXYZRGBNormal & p1 = ret->points[i]; +// pcl::PointXYZRGB & p = cloud->points[i]; +// pcl::Normal & pn = normals->points[i]; + +// p1.x = p.x; +// p1.y = p.y; +// p1.z = p.z; +//// p1.r = rc; +//// p1.g = gc; +//// p1.b = bc; +// p1.r = p.r; +// p1.g = p.g; +// p1.b = p.b; +// p1.normal_x = pn.normal_x; +// p1.normal_y = pn.normal_y; +// p1.normal_z = pn.normal_z; +// } +/* + +*/ + +// for(int i = 0; i < ret->points.size(); i++){ +// pcl::PointXYZRGBNormal & p1 = ret->points[i]; +// p1.r = rc; +// p1.g = gc; +// p1.b = bc; +// } + return ret; +} + +int nr_vAdditionalViews = 4; + +std::vector models; + +reglib::Model * load2(std::string sweep_xml){ + int slash_pos = sweep_xml.find_last_of("/"); + std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; + printf("folder: %s\n",sweep_folder.c_str()); + + SimpleXMLParser parser; + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); + + reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS + cam->fx = 536.458000; + cam->fy = 537.422000; + cam->cx = 314.458000; + cam->cy = 242.038000; + + cv::Mat fullmask; + fullmask.create(480,640,CV_8UC1); + unsigned char * maskdata = (unsigned char *)fullmask.data; + for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} + + reglib::Model * sweepmodel = 0; + std::vector current_room_frames; + for (size_t i=0; ipoints.size()<frames.push_back(frame); + sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose); + sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); + } + } + sweepmodel->recomputeModelPoints(); + models.push_back(sweepmodel); + printf("nr points: %i\n",sweepmodel->points.size()); + +/* + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( objectModel, reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + objectModel->print(); + mu->makeInitialSetup(); + objectModel->print(); + delete mu; + + objectModel->recomputeModelPoints(); +*/ +// reglib::Model * newmodelHolder = new reglib::Model(); +// newmodelHolder->submodels.push_back(sweepmodel); +// newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); +// newmodelHolder->recomputeModelPoints(); +/* + pcl::PointCloud::Ptr cn1 = objectModel->getPCLnormalcloud(1, false); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cn1, pcl::visualization::PointCloudColorHandlerRGBField(cn1), "sample cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); + viewer->spin(); +*/ + pcl::PointCloud::Ptr cn = sweepmodel->getPCLnormalcloud(1, false); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cn, pcl::visualization::PointCloudColorHandlerRGBField(cn), "sample cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); + viewer->spin(); + + return sweepmodel; +} + +void load(std::string sweep_xml){ + + int slash_pos = sweep_xml.find_last_of("/"); + std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; + printf("folder: %s\n",sweep_folder.c_str()); + + SimpleXMLParser parser; + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); + vector objects = semantic_map_load_utilties::loadAllDynamicObjectsFromSingleSweep(sweep_folder+"room.xml"); + + int object_id; + for (unsigned int i=0; i< objects.size(); i++){ + if (objects[i].objectScanIndices.size() != 0){object_id = i;} + } + int index = objectFiles[object_id].toStdString().find_last_of("."); + string object_root_name = objectFiles[object_id].toStdString().substr(0,index); + + for (auto object : objects){ + if (!object.objectScanIndices.size()){continue;} + + nr_vAdditionalViews = 1 + rand()%5; + + std::vector viewrgbs; + std::vector viewdepths; + std::vector viewmasks; + std::vector viewtfs; + std::vector viewposes; + std::vector< image_geometry::PinholeCameraModel > viewcams; + char buf [1024]; + sprintf(buf,"%s/object_%i/poses.txt",sweep_folder.c_str(),object_id); + std::vector poses = getRegisteredViewPoses(std::string(buf), object.vAdditionalViews.size()); + cout<<"Loaded "<height,cloud->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)mask.data; + + cv::Mat rgb; + rgb.create(cloud->height,cloud->width,CV_8UC3); + unsigned char * rgbdata = (unsigned char *)rgb.data; + + cv::Mat depth; + depth.create(cloud->height,cloud->width,CV_16UC1); + unsigned short * depthdata = (unsigned short *)depth.data; + + unsigned int nr_data = cloud->height * cloud->width; + for(unsigned int j = 0; j < nr_data; j++){ + maskdata[j] = 0; + PointType p = cloud->points[j]; + rgbdata[3*j+0] = p.b; + rgbdata[3*j+1] = p.g; + rgbdata[3*j+2] = p.r; + depthdata[j] = short(5000.0 * p.z); + } + + cout<<"Processing AV "<>av_index; + av_mask->push_back(object.vAdditionalViews[i]->points[av_index]); + maskdata[av_index] = 255; + } + + viewrgbs.push_back(rgb); + viewdepths.push_back(depth); + viewmasks.push_back(mask); + viewtfs.push_back(object.vAdditionalViewsTransforms[i]); + viewposes.push_back(poses[i+1]); + viewcams.push_back(roomData.vIntermediateRoomCloudCamParams.front()); + + cv::namedWindow("rgbimage", cv::WINDOW_AUTOSIZE); + cv::imshow( "rgbimage", rgb); + cv::namedWindow("depthimage", cv::WINDOW_AUTOSIZE); + cv::imshow( "depthimage", depth); + cv::namedWindow("mask", cv::WINDOW_AUTOSIZE); + cv::imshow( "mask", mask); + cv::waitKey(30); + } + + if(viewrgbs.size() > 0){ + + rgbs.push_back(viewrgbs); + depths.push_back(viewdepths); + masks.push_back(viewmasks); + tfs.push_back(viewtfs); + initposes.push_back(viewposes); + cams.push_back(viewcams); +/* + tf::StampedTransform tf = roomData.vIntermediateRoomCloudTransforms.front(); + geometry_msgs::TransformStamped tfstmsg; + tf::transformStampedTFToMsg (tf, tfstmsg); + geometry_msgs::Transform tfmsg = tfstmsg.transform; + geometry_msgs::Pose pose; + pose.orientation = tfmsg.rotation; + pose.position.x = tfmsg.translation.x; + pose.position.y = tfmsg.translation.y; + pose.position.z = tfmsg.translation.z; + Eigen::Affine3d localthing; + tf::poseMsgToEigen(pose, localthing); + + reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS + cam->fx = viewcams.front().fx(); + cam->fy = viewcams.front().fy(); + cam->cx = viewcams.front().cx(); + cam->cy = viewcams.front().cy(); + + + cv::Mat fullmask; + fullmask.create(480,640,CV_8UC1); + unsigned char * maskdata = (unsigned char *)fullmask.data; + for(int j = 0; j < 480*640; j++){maskdata[j] = 0;} + + + reglib::Model * objectModel = 0;//new reglib::Model(frames[frame_id],mask); + + std::vector current_frames; + for (unsigned int i=0;i < nr_vAdditionalViews && iframes.push_back(frame); + objectModel->relativeposes.push_back(current_frames.front()->pose.inverse() * frame->pose); + objectModel->modelmasks.push_back(new reglib::ModelMask(viewmasks[i]));//fullmask)); + } + } + + reglib::Model * sweepmodel = 0; + std::vector current_room_frames; + for (size_t i=0; ipoints.size()<frames.push_back(frame); + sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose); + sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); + } + } + sweepmodel->recomputeModelPoints(); + +// objectModel->submodels.push_back(sweepmodel); +// objectModel->submodels_relativeposes.push_back(objectModel->frames.front()->pose.inverse() * sweepmodel->frames.front()->pose); + + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( objectModel, reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + objectModel->print(); + mu->makeInitialSetup(); + objectModel->print(); + delete mu; + + objectModel->recomputeModelPoints(); + + reglib::Model * newmodelHolder = new reglib::Model(); + newmodelHolder->submodels.push_back(objectModel); + newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + newmodelHolder->recomputeModelPoints(); + + pcl::PointCloud::Ptr cn1 = objectModel->getPCLnormalcloud(1, false); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cn1, pcl::visualization::PointCloudColorHandlerRGBField(cn1), "sample cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); + viewer->spin(); + + pcl::PointCloud::Ptr cn = sweepmodel->getPCLnormalcloud(1, false); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cn, pcl::visualization::PointCloudColorHandlerRGBField(cn), "sample cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); + viewer->spin(); +*/ +// rgbs.push_back(viewrgbs); +// depths.push_back(viewdepths); +// masks.push_back(viewmasks); +// tfs.push_back(viewtfs); +// initposes.push_back(viewposes); +// cams.push_back(viewcams); + } + } +} + +int main(int argc, char** argv){ + + ros::init(argc, argv, "use_rares_client"); + ros::NodeHandle n; + ros::Subscriber sub = n.subscribe("modelserver", 1000, chatterCallback); + + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + ros::NodeHandle pn("~"); + for(int ar = 1; ar < argc; ar++){ + string overall_folder = std::string(argv[ar]); + + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + for (auto sweep_xml : sweep_xmls) { + load2(sweep_xml); + } + } + + ros::spin(); +} diff --git a/quasimodo/quasimodo_brain/src/velodyne2.cpp b/quasimodo/quasimodo_brain/src/velodyne2.cpp index 82b11b2b..eb2df121 100644 --- a/quasimodo/quasimodo_brain/src/velodyne2.cpp +++ b/quasimodo/quasimodo_brain/src/velodyne2.cpp @@ -18,63 +18,364 @@ #include "modelupdater/ModelUpdater.h" #include "core/RGBDFrame.h" + +#include + +#include +#include +#include +#include +#include +#include + #include using namespace std; +boost::shared_ptr viewer; + int counterr = 0; int counterl = 0; std::string path = "./"; +//From crossproduct +void getNormal(float & nx, float & ny, float & nz, float x1, float y1, float z1,float x2, float y2, float z2,float x3, float y3, float z3){ + float u1 = x1-x2; + float u2 = y1-y2; + float u3 = z1-z2; + // float unorm = sqrt(u1*u1+u2*u2+u3*u3); + // u1 /= unorm; + // u2 /= unorm; + // u3 /= unorm; + + float v1 = x1-x3; + float v2 = y1-y3; + float v3 = z1-z3; + // float vnorm = sqrt(v1*v1+v2*v2+v3*v3); + // v1 /= vnorm; + // v2 /= vnorm; + // v3 /= vnorm; + + + //Corssprod u x v + nx = u2*v3-u3*v2; + ny = u3*v1-u1*v3; + nz = u1*v2-u2*v1; + + float nnorm = sqrt(nx*nx+ny*ny+nz*nz); + nx /= nnorm; + ny /= nnorm; + nz /= nnorm; + + //printf("%f %f %f\n",nx,ny,nz); + //Flip direction if point + normal further away than point + if(((x1+nx)*(x1+nx) + (y1+ny)*(y1+ny) + (z1+nz)*(z1+nz)) > (x1*x1 + y1*y1 + z1*z1)){ + nx = -nx; + ny = -ny; + nz = -nz; + } + + +} + +double scoren (Eigen::Vector3d & a, Eigen::Vector3d & b){ + double v = a.dot(b); + if(std::isnan(v)){return 0;} + return v*v; +} + +unsigned int text_id = 0; +bool gonext = false; +void keyboardEventOccurred (const pcl::visualization::KeyboardEvent &event, void* viewer_void) { + boost::shared_ptr viewer = *static_cast *> (viewer_void); + if (event.getKeySym () == "n" && event.keyDown ()){ + gonext = true; + std::cout << "n was pressed => removing all text" << std::endl; + } +} + +void fillNormals(pcl::PointCloud & cloud, int normaltype = 0){ + viewer->registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer); + int stepw = 2*16; + + unsigned int nrp = cloud.points.size(); + for(int i = 0; i < cloud.points.size(); i++){ + pcl::PointXYZRGBNormal & np = cloud.points[i]; + + pcl::PointXYZRGBNormal & right = cloud.points[(i+stepw)%nrp]; + pcl::PointXYZRGBNormal & left = cloud.points[(nrp+i-stepw)%nrp]; + pcl::PointXYZRGBNormal up; + if(i%16 != 0){up = cloud.points[i-1];} + + pcl::PointXYZRGBNormal down; + if(i%16 != 15){down = cloud.points[i+1];} + + Eigen::Vector3d npv (np.x,np.y,np.z); + Eigen::Vector3d rv (right.x,right.y,right.z); + Eigen::Vector3d uv (up.x,up.y,up.z); + Eigen::Vector3d lv (left.x,left.y,left.z); + Eigen::Vector3d dv (down.x,down.y,down.z); + + Eigen::Vector3d n1 = (npv-rv).cross(npv-uv); + Eigen::Vector3d n2 = (npv-rv).cross(npv-dv); + Eigen::Vector3d n3 = (npv-lv).cross(npv-uv); + Eigen::Vector3d n4 = (npv-lv).cross(npv-dv); + + n1.normalize(); + n2.normalize(); + n3.normalize(); + n4.normalize(); + + double s1 = scoren(n1,n1)+scoren(n1,n2)+scoren(n1,n3)+scoren(n1,n4); + double s2 = scoren(n2,n1)+scoren(n2,n2)+scoren(n2,n3)+scoren(n2,n4); + double s3 = scoren(n3,n1)+scoren(n3,n2)+scoren(n3,n3)+scoren(n3,n4); + double s4 = scoren(n4,n1)+scoren(n4,n2)+scoren(n4,n3)+scoren(n4,n4); + + Eigen::Vector3d n = n1; + double score = s1; + if(s2 > score){score = s2; n = n2;} + if(s3 > score){score = s3; n = n3;} + if(s4 > score){score = s4; n = n4;} + + if((npv+n).norm() > npv.norm()){ + n = -n; + } + + np.normal_x = n(0); + np.normal_y = n(1); + np.normal_z = n(2); + } +} + +pcl::PointCloud getCloudWithNormals(pcl::PointCloud & cloud, int normaltype = 0){ + pcl::PointCloud normalcloud; + normalcloud.points.resize(cloud.points.size()); + for(int i = 0; i < cloud.points.size(); i++){ + pcl::PointXYZRGB & op = cloud.points[i]; + pcl::PointXYZRGBNormal & np = normalcloud.points[i]; + np.r = 0;//op.r; + np.g = 255;//op.g; + np.b = 0;//op.b; + np.x = op.x; + np.y = op.y; + np.z = op.z; + } + + // pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); + // *cloud_ptr = normalcloud; + // viewer->removeAllPointClouds(); + // viewer->addPointCloud (cloud_ptr, pcl::visualization::PointCloudColorHandlerRGBField(cloud_ptr), "cloud"); + // viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud"); + // //viewer->addPointCloudNormals (cloud_ptr, 17, 1.5, "normals"); + // viewer->spin(); + fillNormals(normalcloud,normaltype); + return normalcloud; +} + + +pcl::PointCloud getSparsifyCloud(pcl::PointCloud & cloud, int stepw){ + pcl::PointCloud sparsecloud; + sparsecloud.reserve(cloud.points.size()/stepw); + for(int i = 0; i < cloud.points.size(); i+= 16*5){//16*(stepw-1)){ + int starti = i; + for(;i < starti+16; i++){ + sparsecloud.push_back(cloud.points[i]); + } + } + return sparsecloud; +} + +pcl::PointCloud getCloudFromParts(pcl::PointCloud prev, pcl::PointCloud curr, Eigen::Matrix4d motion = Eigen::Matrix4d::Identity()){ + pcl::PointCloud prev_cloud; + prev_cloud.resize(prev.points.size()/2); + for(int i = 0; i < prev.points.size(); i+=2){ + prev_cloud.points[i/2] = prev.points[i]; + } + + pcl::PointCloud curr_cloud; + curr_cloud.resize(curr.points.size()/2); + for(int i = 0; i < prev.points.size(); i+=2){ + curr_cloud.points[i/2] = curr.points[i]; + } + + + pcl::PointCloud fullcloud = prev_cloud+curr_cloud; + + Eigen::Matrix4d invmotion = motion.inverse(); + Eigen::Matrix3d rot = invmotion.block(0,0,3,3); + Vector3d ea = rot.eulerAngles(2, 0, 2); + + for(int i = 0; i < fullcloud.points.size(); i+=16){ + double part = double(i)/double(fullcloud.points.size()); + + + Eigen::Affine3d mat; + mat = Eigen::AngleAxisd(part*ea(0), Eigen::Vector3d::UnitZ())*Eigen::AngleAxisd(part*ea(1), Eigen::Vector3d::UnitX())*Eigen::AngleAxisd(part*ea(2), Eigen::Vector3d::UnitZ()); + + const double & m00 = mat(0,0); const double & m01 = mat(0,1); const double & m02 = mat(0,2); const double & m03 = part*invmotion(0,3); + const double & m10 = mat(1,0); const double & m11 = mat(1,1); const double & m12 = mat(1,2); const double & m13 = part*invmotion(1,3); + const double & m20 = mat(2,0); const double & m21 = mat(2,1); const double & m22 = mat(2,2); const double & m23 = part*invmotion(2,3); + + for(int j = i; j < i+16; j++){//TODO set zeros to zero still + pcl::PointXYZRGB & p = fullcloud.points[j]; + const double & src_x = p.x; + const double & src_y = p.y; + const double & src_z = p.z; + p.x = m00*src_x + m01*src_y + m02*src_z + m03; + p.y = m10*src_x + m11*src_y + m12*src_z + m13; + p.z = m20*src_x + m21*src_y + m22*src_z + m23; + } + } + + pcl::PointCloud fullcloudnormals = getCloudWithNormals(fullcloud,0); + return fullcloudnormals; +} + +pcl::PointCloud prevl; +std::vector< pcl::PointCloud > all_cloudsl; +std::vector< Eigen::Matrix4d > all_posesl; void cloud_cb_l(const sensor_msgs::PointCloud2ConstPtr& input){ - ROS_INFO("l pointcloud in %i",counterl); + ROS_INFO("l pointcloud in %i",counterl); + + pcl::PointCloud tmpcloud; + pcl::fromROSMsg (*input, tmpcloud); + + pcl::PointCloud cloud; + cloud.resize(tmpcloud.points.size()/2); + for(int i = 0; i < tmpcloud.points.size(); i+=2){cloud.points[i/2] = tmpcloud.points[i];} + + + char buf[1024]; + if(counterl % 2 == 1){ + pcl::PointCloud fullcloudnormals = getCloudFromParts(prevl,cloud, Eigen::Matrix4d::Identity()); + pcl::PointCloud sparsecloudnormals = getSparsifyCloud(fullcloudnormals,6); +// all_clouds.push_back(fullcloudnormals); +// if(all_poses.size()==0){all_poses.push_back(Eigen::Matrix4d::Identity()); +// }else if(all_poses.size()==1){all_poses.push_back(all_poses.back()); +// }else{ +// all_poses.push_back(all_poses.back()*all_poses[all_poses.size()].inverse()*all_poses.back()); +// } + +// reglib::MassRegistrationPPR * massreg = new reglib::MassRegistrationPPR(0.1); +// massreg->timeout = 60; +// massreg->viewer = viewer; +// massreg->visualizationLvl = 1; + +//// massreg->setData(clouds); + +// massreg->stopval = 0.001; +// massreg->steps = 10; + +//// reglib::MassFusionResults mfr = massreg->getTransforms(relativeposes); +// delete massreg; + +// pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); +// *cloud_ptr = fullcloudnormals; +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud_ptr, pcl::visualization::PointCloudColorHandlerRGBField(cloud_ptr), "cloud"); +// viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud"); +// viewer->addPointCloudNormals (cloud_ptr, 17, 0.5, "normals"); +// viewer->spinOnce(); + + sprintf(buf,"%s/normalsfullleft_%.10i.pcd",path.c_str(),counterl/2); + pcl::io::savePCDFileBinary (string(buf), fullcloudnormals); + sprintf(buf,"%s/normalssparseleft_%.10i.pcd",path.c_str(),counterl/2); + pcl::io::savePCDFileBinary (string(buf), sparsecloudnormals); + } + + prevl = cloud; + counterl++; } +pcl::PointCloud prevr; +std::vector< pcl::PointCloud::Ptr > all_clouds; +std::vector< Eigen::Matrix4d > all_poses; + void cloud_cb_r(const sensor_msgs::PointCloud2ConstPtr& input){ - ROS_INFO("r pointcloud in %i",counterr); -// if(counter % 5 == 4){ -// pcl::PointCloud cloud; -// pcl::fromROSMsg (*input, cloud); -// //addAndUpdate(cloud); -// char buf[1024]; -// sprintf(buf,"%s%.10i.pcd",path.c_str(),counter); -// printf("saving: %s\n",buf); -// pcl::io::savePCDFileBinary (string(buf), cloud); - -// cv::Mat rgb; -// rgb.create(cloud.height,cloud.width,CV_8UC3); -// unsigned char * rgbdata = (unsigned char *)rgb.data; -// unsigned int nr_data = cloud.height * cloud.width; -// for(unsigned int i = 0; i < nr_data; i++){ -// pcl::PointXYZRGB p = cloud.points[i]; -// rgbdata[3*i+0] = p.b; -// rgbdata[3*i+1] = p.g; -// rgbdata[3*i+2] = p.r; -// } -// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "rgb", rgb ); -// cv::waitKey(30); -// } -// counter++; + ROS_INFO("r pointcloud in %i",counterr); + + pcl::PointCloud tmpcloud; + pcl::fromROSMsg (*input, tmpcloud); + + pcl::PointCloud cloud; + cloud.resize(tmpcloud.points.size()/2); + for(int i = 0; i < tmpcloud.points.size(); i+=2){cloud.points[i/2] = tmpcloud.points[i];} + + + char buf[1024]; + if(counterr % 2 == 1){ + pcl::PointCloud fullcloudnormals = getCloudFromParts(prevr,cloud, Eigen::Matrix4d::Identity()); + pcl::PointCloud sparsecloudnormals = getSparsifyCloud(fullcloudnormals,6); + + pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); + *cloud_ptr = fullcloudnormals; + all_clouds.push_back(cloud_ptr); + if(all_poses.size()<=1){all_poses.push_back(Eigen::Matrix4d::Identity()); + }else{ + Eigen::Matrix4d v = all_poses[all_poses.size()-2].inverse()*all_poses.back(); + all_poses.push_back(v*all_poses.back()); + } + if(all_poses.size()>2){ + + std::vector< pcl::PointCloud::Ptr > ac; + ac.push_back(all_clouds.front()); + ac.push_back(all_clouds.back()); + + std::vector< Eigen::Matrix4d > ap; + ap.push_back(all_poses.front()); + ap.push_back(all_poses.back()); + reglib::MassRegistrationPPR * massreg = new reglib::MassRegistrationPPR(0.1); + massreg->timeout = 60; + massreg->viewer = viewer; + massreg->visualizationLvl = 0; + if(all_poses.size()%10 == 0){massreg->visualizationLvl = 1;} + + + massreg->setData(ac); + + massreg->stopval = 0.001; + massreg->steps = 10; + + reglib::MassFusionResults mfr = massreg->getTransforms(ap); + all_poses.back() = mfr.poses.back(); + delete massreg; + } + + + +// pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); +// *cloud_ptr = fullcloudnormals; +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud_ptr, pcl::visualization::PointCloudColorHandlerRGBField(cloud_ptr), "cloud"); +// viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud"); +// viewer->addPointCloudNormals (cloud_ptr, 17, 0.5, "normals"); +// viewer->spinOnce(); + + sprintf(buf,"%s/normalssparseright_%.10i.pcd",path.c_str(),counterr/2); + pcl::io::savePCDFileBinary (string(buf), sparsecloudnormals); + sprintf(buf,"%s/normalsfullright_%.10i.pcd",path.c_str(),counterr/2); + pcl::io::savePCDFileBinary (string(buf), fullcloudnormals); + } + + prevr = cloud; + counterr++; } int main(int argc, char **argv){ - boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); viewer->addCoordinateSystem(); viewer->setBackgroundColor(0.8,0.8,0.8); + viewer->initCameraParameters (); - string pathr; - if(argc < 2){ pathr = "/camera/depth_registered/points";} - else{ pathr = string(argv[1]);} - - string pathl; - if(argc < 3){ pathl = "/camera/depth_registered/points";} - else{ pathl = string(argv[2]);} + string pathr = "/velodyne_points_right"; + string pathl = "/velodyne_points_left"; + if(argc > 1){ pathr = string(argv[1]);} + if(argc > 2){ pathl = string(argv[2]);} - ros::init(argc, argv, "massreg_velodyne_node"); + ros::init(argc, argv, "massregVelodyneNode"); ros::NodeHandle n; - ros::Subscriber subr = n.subscribe (path, 0, cloud_cb_r); - ros::Subscriber subl = n.subscribe (path, 0, cloud_cb_l); + ros::Subscriber subr = n.subscribe (pathr, 0, cloud_cb_r); + ros::Subscriber subl = n.subscribe (pathl, 0, cloud_cb_l); ros::spin(); return 0; diff --git a/quasimodo/quasimodo_brain/src/voPCD.cpp b/quasimodo/quasimodo_brain/src/voPCD.cpp new file mode 100644 index 00000000..a7c49022 --- /dev/null +++ b/quasimodo/quasimodo_brain/src/voPCD.cpp @@ -0,0 +1,88 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include "std_msgs/String.h" + +#include "eigen_conversions/eigen_msg.h" +#include "tf_conversions/tf_eigen.h" +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" +#include "quasimodo_msgs/retrieval_query_result.h" +#include "quasimodo_msgs/retrieval_query.h" +#include "quasimodo_msgs/retrieval_result.h" +#include "soma2_msgs/SOMA2Object.h" + +#include "soma_manager/SOMA2InsertObjs.h" + +//#include "modelupdater/ModelUpdater.h" +//#include "/home/johane/catkin_ws_dyn/src/quasimodo_models/include/modelupdater/ModelUpdater.h" +#include "modelupdater/ModelUpdater.h" +#include "core/RGBDFrame.h" +#include +#include + +#include "metaroom_xml_parser/simple_xml_parser.h" +#include "metaroom_xml_parser/simple_summary_parser.h" + +#include +#include +#include +#include +#include + +#include + +#include "ModelDatabase/ModelDatabase.h" + +#include + +#include + +#include +#include +#include +#include +#include +#include + + +boost::shared_ptr viewer; + +int main(int argc, char **argv){ + boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); + viewer->addCoordinateSystem(0.1); + viewer->setBackgroundColor(0.9,0.9,0.9); + + std::vector< pcl::PointCloud::Ptr > clouds; + std::vector relativeposes; + for(int arg = 1; arg < argc; arg++){ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + + if (pcl::io::loadPCDFile (argv[arg], *cloud) == -1){ + printf ("Error: Couldn't read file %s\n",argv[arg]); + return (-1); + } + clouds.push_back(cloud); + relativeposes.push_back(Eigen::Matrix4d::Identity()); + } + + + reglib::MassRegistrationPPR * massreg = new reglib::MassRegistrationPPR(0.1); + massreg->timeout = 60; + massreg->viewer = viewer; + massreg->visualizationLvl = 1; + + massreg->setData(clouds); + + massreg->stopval = 0.00001; + massreg->steps = 10; + + reglib::MassFusionResults mfr = massreg->getTransforms(relativeposes); + + return 0; +} diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 3d16a0b5..9e33fc9d 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(quasimodo_models) #find_package(catkin REQUIRED)# quasimodo_msgs)# COMPONENTS quasimodo_msgs) -set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fPIC -std=c++0x -o popcnt -mssse3") +set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") find_package(catkin REQUIRED COMPONENTS @@ -14,7 +14,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include - LIBRARIES quasimodo_ModelUpdater quasimodo_reglib quasimodo_massreglib quasimodo_model quasimodo_core quasimodo_Mesher quasimodo_weightlib quasimodo_modelmask + LIBRARIES quasimodo_ModelUpdater quasimodo_SICP quasimodo_reglib quasimodo_massreglib quasimodo_model quasimodo_core quasimodo_myhull quasimodo_Mesher quasimodo_weightlib quasimodo_modelmask # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib ) @@ -62,15 +62,15 @@ target_link_libraries(quasimodo_core ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} ${ca add_library(quasimodo_reglib src/registration/Registration.cpp src/registration/RegistrationRandom.cpp src/registration/RegistrationRefinement.cpp) target_link_libraries(quasimodo_reglib quasimodo_weightlib ceres glog ${catkin_LIBRARIES}) -add_library(quasimodo_massreglib src/registration/MassRegistration.cpp src/registration/MassRegistrationPPR.cpp) -target_link_libraries(quasimodo_massreglib quasimodo_reglib quasimodo_core) - add_library(quasimodo_modelmask src/model/ModelMask.cpp) target_link_libraries(quasimodo_modelmask quasimodo_core quasimodo_reglib ${catkin_LIBRARIES}) add_library(quasimodo_model src/model/Model.cpp) target_link_libraries(quasimodo_model quasimodo_modelmask quasimodo_core quasimodo_reglib ${catkin_LIBRARIES}) +add_library(quasimodo_massreglib src/registration/MassRegistration.cpp src/registration/MassRegistrationPPR.cpp src/registration/MassRegistrationPPR2.cpp) +target_link_libraries(quasimodo_massreglib quasimodo_model quasimodo_reglib quasimodo_core) + add_library(quasimodo_Mesher src/mesher/Mesh.cpp) target_link_libraries(quasimodo_Mesher quasimodo_model ${catkin_LIBRARIES}) diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 405ea371..052091a0 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -16,6 +16,10 @@ #include #include +#include +#include +#include + #include "Util.h" #include "Camera.h" @@ -23,6 +27,7 @@ namespace reglib { class RGBDFrame{ public: + std::string keyval; Camera * camera; unsigned long id; double capturetime; diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index 066d7803..e55ef7ad 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -140,6 +140,7 @@ template struct ArrayData3D { double getTime(); + struct PointCostFunctor { double m1 [3]; double m2 [3]; @@ -230,7 +231,7 @@ template struct ArrayData3D { Eigen::Matrix & Y, Eigen::Matrix & Yn, Eigen::VectorXd & W); - bool isconverged(std::vector before, std::vector after, double stopvalr = 0.001, double stopvalt = 0.001); + bool isconverged(std::vector before, std::vector after, double stopvalr = 0.001, double stopvalt = 0.001, bool verbose = false); void setFirstIdentity(std::vector & v); } diff --git a/quasimodo/quasimodo_models/include/model/Model.h b/quasimodo/quasimodo_models/include/model/Model.h index 21a90dae..2ed9bbb6 100644 --- a/quasimodo/quasimodo_models/include/model/Model.h +++ b/quasimodo/quasimodo_models/include/model/Model.h @@ -20,7 +20,8 @@ namespace reglib { - +using namespace std; +using namespace Eigen; class superpoint{ public: Eigen::Vector3f point; @@ -42,16 +43,24 @@ class superpoint{ ~superpoint(){} void merge(superpoint p, double weight = 1){ - point = weight*p.point_information*p.point + point_information*point; - point /= weight*p.point_information + point_information; - point_information = weight*p.point_information + point_information; + double newpweight = weight*p.point_information + point_information; + double newfweight = weight*p.feature_information + feature_information; +//printf("before: (%3.3f)(%3.3f) + (%3.3f)(%3.3f) -> (%3.3f)(%3.3f)\n", +//feature_information,feature(0),p.feature_information,p.feature(0),(feature_information*feature(0)+p.feature_information*p.feature(0))/(feature_information+p.feature_information)); + + point = weight*p.point_information*p.point + point_information*point; + normal = weight*p.point_information*p.normal + point_information*normal; + feature = weight*p.feature_information*p.feature + feature_information*feature; + - normal = weight*p.point_information*p.normal + point_information*normal; normal.normalize(); - feature = weight*p.feature_information*p.feature + feature_information*feature; - feature /= weight*p.feature_information + feature_information; - feature_information = weight*p.feature_information + feature_information; + point /= newpweight; + point_information = newpweight; + + feature /= newfweight; + feature_information = newfweight; + last_update_frame_id = std::max(p.last_update_frame_id,last_update_frame_id); } }; @@ -66,21 +75,34 @@ class superpoint{ std::vector points; - std::vector relativeposes; - std::vector frames; - //std::vector masks; - std::vector modelmasks; + std::vector< std::vector > all_keypoints; + std::vector< cv::Mat > all_descriptors; + std::vector relativeposes; + std::vector frames; + std::vector modelmasks; + + std::vector rep_relativeposes; + std::vector rep_frames; + std::vector rep_modelmasks; double total_scores; std::vector > scores; + std::vector submodels; + std::vector submodels_relativeposes; + std::vector > submodels_scores; + Model(); Model(RGBDFrame * frame_, cv::Mat mask, Eigen::Matrix4d pose = Eigen::Matrix4d::Identity()); ~Model(); void merge(Model * model, Eigen::Matrix4d p); - void recomputeModelPoints(); + void showHistory(boost::shared_ptr viewer); + + void addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask); + void addAllSuperPoints(vector & spvec, Eigen::Matrix4d pose); + void recomputeModelPoints(Eigen::Matrix4d pose = Eigen::Matrix4d::Identity()); void addPointsToModel(RGBDFrame * frame, ModelMask * modelmask, Eigen::Matrix4d p); //void addFrameToModel(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d p); diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 3b098a42..7c2d4283 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -13,11 +13,19 @@ #include "../registration/MassRegistration.h" #include "../mesher/Mesh.h" +#include +#include +#include +#include +#include + #include namespace reglib { + using namespace std; + using namespace Eigen; class UpdatedModels{ public: std::vector< Model * > new_models; @@ -36,6 +44,13 @@ namespace reglib OcclusionScore(){score = 0;occlusions = 0;} OcclusionScore( double score_ ,double occlusions_){score = score_;occlusions = occlusions_;} ~OcclusionScore(){} + + void add(OcclusionScore oc){ + score += oc.score; + occlusions += oc.occlusions; + } + + void print(){printf("score: %5.5f occlusions: %5.5f\n",score,occlusions);} }; class ModelUpdater{ @@ -54,7 +69,30 @@ namespace reglib virtual FusionResults registerModel(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1); virtual void fuse(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1); virtual UpdatedModels fuseData(FusionResults * f, Model * model1,Model * model2); - virtual void refine(double reg = 0.05,bool useFullMask = false); + + virtual bool isRefinementNeeded(); + virtual bool refineIfNeeded(); + + virtual void makeInitialSetup(); + + virtual double getCompareUtility(Matrix4d p, RGBDFrame* frame, ModelMask* mask, vector & cp, vector & cf, vector & cm); + virtual void getGoodCompareFrames(vector & cp, vector & cf, vector & cm); + + virtual pcl::PointCloud::Ptr getPCLnormalcloud(vector & points); + virtual void addSuperPoints(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int type = 1, bool debugg = false); + virtual vector getSuperPoints(vector cp, vector cf, vector cm, int type = 1, bool debugg = false); + + + virtual OcclusionScore computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1, bool debugg = false); + virtual OcclusionScore computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp = Matrix4d::Identity(), int step = 1, bool debugg = false); + virtual OcclusionScore computeOcclusionScore(Model * model1, Model * model2, Matrix4d rp = Matrix4d::Identity(),int step = 1, bool debugg = false); + virtual vector > computeOcclusionScore(vector models, vector rps, int step = 1, bool debugg = false); + + virtual double computeOcclusionScoreCosts(vector models); + + virtual void addModelsToVector(vector & models, vector & rps, Model * model, Matrix4d rp); + + virtual void refine(double reg = 0.05,bool useFullMask = false, int visualization = 0); virtual void show(bool stop = true); virtual void pruneOcclusions(); // virtual OcclusionScore computeOcclusionScore(RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step = 1, bool debugg = false); diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistration.h b/quasimodo/quasimodo_models/include/registration/MassRegistration.h index d5fe2634..04074ceb 100644 --- a/quasimodo/quasimodo_models/include/registration/MassRegistration.h +++ b/quasimodo/quasimodo_models/include/registration/MassRegistration.h @@ -73,6 +73,15 @@ namespace reglib MassRegistration(); ~MassRegistration(); + bool okVal(double v); + bool isValidPoint(pcl::PointXYZRGBNormal p); + + + virtual void clearData(); + virtual void addData(RGBDFrame* frame, ModelMask * mmask); + virtual void addModelData(Model * model, bool submodels = true); + virtual void addData(pcl::PointCloud::Ptr cloud); + virtual void setData(std::vector frames_, std::vector mmasks); virtual void setData(std::vector< pcl::PointCloud::Ptr > all_clouds); void setVisualizationLvl(unsigned int lvl); @@ -86,5 +95,6 @@ namespace reglib } #include "MassRegistrationPPR.h" +#include "MassRegistrationPPR2.h" #endif // MassRegistration_H diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR.h b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR.h index fb436d05..bd491369 100644 --- a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR.h +++ b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR.h @@ -21,6 +21,23 @@ namespace reglib double stopval; unsigned steps; + double rematch_time; + double residuals_time; + double opt_time; + double computeModel_time; + double setup_matches_time; + double setup_equation_time; + double setup_equation_time2; + double solve_equation_time; + double total_time_start; + + const unsigned int maxcount = 1000000; + double * Qp_arr; + double * Qn_arr; + double * Xp_arr; + double * Xn_arr; + double * rangeW_arr; + std::vector nr_datas; std::vector< bool > is_ok; @@ -31,8 +48,13 @@ namespace reglib std::vector< Eigen::Matrix > transformed_normals; std::vector< Eigen::VectorXd > informations; - std::vector< int > nr_arraypoints; + std::vector< int > nr_arraypoints; std::vector< double * > arraypoints; + std::vector< double * > arraynormals; + std::vector< double * > arraycolors; + std::vector< double * > arrayinformations; + + std::vector< Tree3d * > trees3d; std::vector< ArrayData3D * > a3dv; @@ -61,10 +83,19 @@ namespace reglib MassRegistrationPPR(double startreg = 0.05, bool visualize = false); ~MassRegistrationPPR(); + void addModelData(Model * model, bool submodels = true); + + void clearData(); + void addData(RGBDFrame* frame, ModelMask * mmask); + void addData(pcl::PointCloud::Ptr cloud); + MassFusionResults getTransforms(std::vector guess); void setData(std::vector frames_, std::vector mmasks); void setData(std::vector< pcl::PointCloud::Ptr > all_clouds); + void rematch(std::vector poses, std::vector prev_poses, bool first); + Eigen::MatrixXd getAllResiduals(std::vector poses); + std::vector optimize(std::vector poses); }; } diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h new file mode 100644 index 00000000..fa0c69ed --- /dev/null +++ b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h @@ -0,0 +1,162 @@ +#ifndef MassRegistrationPPR2_H +#define MassRegistrationPPR2_H + +#include "MassRegistration.h" +#include + +namespace reglib +{ + + class TestMatch{ + public: + + int src; + int dst; + double d; + + TestMatch(int src_, int dst_, double d_){ + src = src_; + dst = dst_; + d = d_; + } + + ~TestMatch(){} + }; + + class MassRegistrationPPR2 : public MassRegistration + { + public: + + MatchType type; + + bool fast_opt; + bool use_PPR_weight; + bool use_features; + bool normalize_matchweights; + + double stopval; + unsigned steps; + + double rematch_time; + double residuals_time; + double opt_time; + double computeModel_time; + double setup_matches_time; + double setup_equation_time; + double setup_equation_time2; + double solve_equation_time; + double total_time_start; + + const unsigned int maxcount = 1000000; + double * Qp_arr; + double * Qn_arr; + double * Xp_arr; + double * Xn_arr; + double * rangeW_arr; + + double * kp_Qp_arr; + double * kp_Qn_arr; + double * kp_Xp_arr; + double * kp_Xn_arr; + double * kp_rangeW_arr; + + double * depthedge_Qp_arr; + double * depthedge_Xp_arr; + double * depthedge_rangeW_arr; + + Model * model; + + std::vector nr_datas; + + std::vector< bool > is_ok; + std::vector< Eigen::Matrix > points; + std::vector< Eigen::Matrix > colors; + std::vector< Eigen::Matrix > normals; + std::vector< Eigen::Matrix > transformed_points; + std::vector< Eigen::Matrix > transformed_normals; + std::vector< Eigen::VectorXd > informations; + + std::vector< int > kp_nr_arraypoints; + std::vector< double * > kp_arraypoints; + std::vector< double * > kp_arraynormals; + std::vector< double * > kp_arrayinformations; + std::vector< uint64_t * > kp_arraydescriptors; + std::vector< std::vector< std::vector< TestMatch > > > kp_matches; + + //std::vector< std::vector< std::vector > > matchids; + + bool use_surface; + std::vector< int > frameid; + + std::vector< int > nr_arraypoints; + std::vector< double * > arraypoints; + std::vector< double * > arraynormals; + std::vector< double * > arraycolors; + std::vector< double * > arrayinformations; + + std::vector< Tree3d * > trees3d; + std::vector< ArrayData3D * > a3dv; + + std::vector nr_matches; + std::vector< std::vector< std::vector > > matchids; + std::vector< std::vector< std::vector > > matchdists; + + + bool use_depthedge; + std::vector< int > depthedge_nr_arraypoints; + std::vector< double * > depthedge_arraypoints; + std::vector< double * > depthedge_arrayinformations; + std::vector< Tree3d * > depthedge_trees3d; + std::vector< ArrayData3D * > depthedge_a3dv; + + std::vector depthedge_nr_matches; + std::vector< std::vector< std::vector > > depthedge_matchids; + std::vector< std::vector< std::vector > > depthedge_matchdists; + + + std::vector sweepids; + std::vector background_nr_datas; +// std::vector< Eigen::Matrix > background_points; +// std::vector< Eigen::Matrix > background_colors; +// std::vector< Eigen::Matrix > background_normals; +// std::vector< Eigen::Matrix > background_transformed_points; +// std::vector< Eigen::Matrix > background_transformed_normals; +// std::vector< Eigen::VectorXd > background_informations; +// std::vector< nanoflann::KDTreeAdaptor * > background_trees; +// std::vector background_nr_matches; +// std::vector< std::vector< std::vector > > background_matchids; + + +// std::vector feature_start;//Dimension of data a specific feature starts, if the feature is RGB this should be 3 +// std::vector feature_end;//Dimension of data a specific feature ends, if the feature is RGB this should be 5 +// std::vector< DistanceWeightFunction2 * > feature_func; + + DistanceWeightFunction2PPR2 * func; + DistanceWeightFunction2PPR2 * kpfunc; + DistanceWeightFunction2PPR2 * depthdege_func; + + MassRegistrationPPR2(double startreg = 0.05, bool visualize = false); + ~MassRegistrationPPR2(); + + void clearData(); + void addModelData(Model * model, bool submodels = true); + void addData(RGBDFrame* frame, ModelMask * mmask); + void addData(pcl::PointCloud::Ptr cloud); + + MassFusionResults getTransforms(std::vector guess); + void setData(std::vector frames_, std::vector mmasks); + void setData(std::vector< pcl::PointCloud::Ptr > all_clouds); + + void rematchKeyPoints(std::vector poses, std::vector prev_poses, bool first); + void rematch(std::vector poses, std::vector prev_poses, bool first); + Eigen::MatrixXd getAllResiduals(std::vector poses); + Eigen::MatrixXd getAllKpResiduals(std::vector poses); + + //Eigen::MatrixXd getAllKPResiduals(std::vector poses); + + std::vector optimize(std::vector poses); + }; + +} + +#endif diff --git a/quasimodo/quasimodo_models/include/registration/Registration.h b/quasimodo/quasimodo_models/include/registration/Registration.h index ead1437f..290286b5 100644 --- a/quasimodo/quasimodo_models/include/registration/Registration.h +++ b/quasimodo/quasimodo_models/include/registration/Registration.h @@ -44,6 +44,7 @@ namespace reglib Eigen::MatrixXd guess; double score; + double stop; bool timeout; std::vector< Eigen::MatrixXd > candidates; diff --git a/quasimodo/quasimodo_models/include/registration/RegistrationRandom.h b/quasimodo/quasimodo_models/include/registration/RegistrationRandom.h index c2d16163..851969d1 100644 --- a/quasimodo/quasimodo_models/include/registration/RegistrationRandom.h +++ b/quasimodo/quasimodo_models/include/registration/RegistrationRandom.h @@ -20,6 +20,8 @@ namespace reglib RegistrationRandom(); ~RegistrationRandom(); + + bool issame(FusionResults fr1, FusionResults fr2, int stepxsmall); FusionResults getTransform(Eigen::MatrixXd guess); }; diff --git a/quasimodo/quasimodo_models/include/registration/RegistrationRefinement.h b/quasimodo/quasimodo_models/include/registration/RegistrationRefinement.h index f34b7979..b7c9f37b 100644 --- a/quasimodo/quasimodo_models/include/registration/RegistrationRefinement.h +++ b/quasimodo/quasimodo_models/include/registration/RegistrationRefinement.h @@ -18,12 +18,11 @@ namespace reglib Eigen::Matrix Y; Eigen::Matrix N; - std::vector total_dweight; unsigned int ycols; Eigen::VectorXd DST_INORMATION; - DistanceWeightFunction2PPR2 * func; + //DistanceWeightFunction2PPR2 * func; int nr_arraypoints; double * arraypoints; diff --git a/quasimodo/quasimodo_models/src/core/2d/convolution.h b/quasimodo/quasimodo_models/src/core/2d/convolution.h new file mode 100644 index 00000000..43e8b991 --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/2d/convolution.h @@ -0,0 +1,159 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_CONVOLUTION_H +#define PCL_2D_CONVOLUTION_H + +#include +#include +#include + +namespace pcl +{ + /** + * This typedef is used to represent a point cloud containing edge information + */ + struct PointXYZIEdge + { + PCL_ADD_POINT4D; // preferred way of adding a XYZ+padding + float magnitude; + float direction; + float magnitude_x; + float magnitude_y; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned + } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment + + /** \brief A 2D convolution class. */ + template + class Convolution : public Filter + { + public: + using Filter::input_; + + /** + * Extra pixels are added to the input image so that convolution can be performed over the entire image. + * + * (kernel_height/2) rows are added before the first row and after the last row + * (kernel_width/2) columns are added before the first column and after the last column + * border options define what values are set for these extra rows and columns + * + * Assume that the three rows of right edge of the image looks like this: + * .. 3 2 1 + * .. 6 5 4 + * .. 9 8 7 + * + * BOUNDARY_OPTION_CLAMP : the extra pixels are set to the pixel value of the boundary pixel + * This option makes it seem as if it were: + * .. 3 2 1| 1 1 1 .. + * .. 6 5 4| 4 4 4 .. + * .. 9 8 7| 7 7 7 .. + * + * BOUNDARY_OPTION_MIRROR : the input image is mirrored at the boundary. + * This option makes it seem as if it were: + * .. 3 2 1| 1 2 3 .. + * .. 6 5 4| 4 5 6 .. + * .. 9 8 7| 7 8 9 .. + * + * BOUNDARY_OPTION_ZERO_PADDING : the extra pixels are simply set to 0 + * This option makes it seem as if it were: + * .. 3 2 1| 0 0 0 .. + * .. 6 5 4| 0 0 0 .. + * .. 9 8 7| 0 0 0 .. + * + * Note that the input image is not actually extended in size. Instead, based on these options, + * the convolution is performed differently at the border pixels. + */ + enum BOUNDARY_OPTIONS_ENUM + { + BOUNDARY_OPTION_CLAMP, + BOUNDARY_OPTION_MIRROR, + BOUNDARY_OPTION_ZERO_PADDING + }; + + Convolution () + { + boundary_options_ = BOUNDARY_OPTION_CLAMP; + } + + /** \brief Sets the kernel to be used for convolution + * \param[in] kernel convolution kernel passed by reference + */ + inline void + setKernel (const pcl::PointCloud &kernel) + { + kernel_ = kernel; + } + + /** + * \param[in] boundary_options enum indicating the boundary options to be used for convolution + */ + inline void + setBoundaryOptions (BOUNDARY_OPTIONS_ENUM boundary_options) + { + boundary_options_ = boundary_options; + } + + /** \brief Performs 2D convolution of the input point cloud with the kernel. + * Uses clamp as the default boundary option. + * \param[out] output Output point cloud passed by reference + */ + void + filter (pcl::PointCloud &output); + + protected: + /** \brief This is an over-ride function for the pcl::Filter interface. */ + void + applyFilter (pcl::PointCloud &) {} + + private: + BOUNDARY_OPTIONS_ENUM boundary_options_; + pcl::PointCloud kernel_; + }; +} + +#include "impl/convolution.hpp" + +POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZIEdge, + (float, x, x) + (float, y, y) + (float, z, z) + (float, magnitude, magnitude) + (float, direction, direction) + (float, magnitude_x, magnitude_x) + (float, magnitude_y, magnitude_y) +) +#endif // PCL_2D_CONVOLUTION_2D_H diff --git a/quasimodo/quasimodo_models/src/core/2d/edge.h b/quasimodo/quasimodo_models/src/core/2d/edge.h new file mode 100644 index 00000000..821f285e --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/2d/edge.h @@ -0,0 +1,306 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_EDGE_H +#define PCL_2D_EDGE_H + +#include +#include "convolution.h" +#include "kernel.h" + +namespace pcl +{ + template + class Edge + { + private: + typedef typename pcl::PointCloud PointCloudIn; + typedef typename PointCloudIn::Ptr PointCloudInPtr; + + PointCloudInPtr input_; + pcl::Convolution convolution_; + kernel kernel_; + + /** \brief This function performs edge tracing for Canny Edge detector. + * + * \param[in] rowOffset row offset for direction in which the edge is to be traced + * \param[in] colOffset column offset for direction in which the edge is to be traced + * \param[in] row row location of the edge point + * \param[in] col column location of the edge point + * \param[out] maxima point cloud containing the edge information in the magnitude channel + */ + inline void + cannyTraceEdge (int rowOffset, int colOffset, int row, int col, + pcl::PointCloud &maxima); + + /** \brief This function discretizes the edge directions in steps of 22.5 degrees. + * \param thet point cloud containing the edge information in the direction channel + */ + void + discretizeAngles (pcl::PointCloud &thet); + + /** \brief This function suppresses the edges which don't form a local maximum + * in the edge direction. + * \param[in] edges point cloud containing all the edges + * \param[out] maxima point cloud containing the non-max supressed edges + * \param[in] tLow + */ + void + suppressNonMaxima (const pcl::PointCloud &edges, + pcl::PointCloud &maxima, float tLow); + + public: + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + enum OUTPUT_TYPE + { + OUTPUT_Y, + OUTPUT_X, + OUTPUT_X_Y, + OUTPUT_MAGNITUDE, + OUTPUT_DIRECTION, + OUTPUT_MAGNITUDE_DIRECTION, + OUTPUT_ALL + }; + + enum DETECTOR_KERNEL_TYPE + { + CANNY, + SOBEL, + PREWITT, + ROBERTS, + LOG, + DERIVATIVE_CENTRAL, + DERIVATIVE_FORWARD, + DERIVATIVE_BACKWARD + }; + + private: + OUTPUT_TYPE output_type_; + DETECTOR_KERNEL_TYPE detector_kernel_type_; + bool non_maximal_suppression_; + bool hysteresis_thresholding_; + + float hysteresis_threshold_low_; + float hysteresis_threshold_high_; + float non_max_suppression_radius_x_; + float non_max_suppression_radius_y_; + + public: + Edge () : + output_type_ (OUTPUT_X), + detector_kernel_type_ (SOBEL), + non_maximal_suppression_ (false), + hysteresis_thresholding_ (false), + hysteresis_threshold_low_ (20), + hysteresis_threshold_high_ (80), + non_max_suppression_radius_x_ (3), + non_max_suppression_radius_y_ (3) + { + } + + /** \brief Set the output type. + * \param[in] output_type the output type + */ + void + setOutputType (OUTPUT_TYPE output_type) + { + output_type_ = output_type; + } + + void + setHysteresisThresholdLow (float threshold) + { + hysteresis_threshold_low_ = threshold; + } + + void + setHysteresisThresholdHigh (float threshold) + { + hysteresis_threshold_high_ = threshold; + } + + /** + * \param[in] input_x + * \param[in] input_y + * \param[out] output + */ + void + sobelMagnitudeDirection (const pcl::PointCloud &input_x, + const pcl::PointCloud &input_y, + pcl::PointCloud &output); + + + /** \brief Perform Canny edge detection with two separated input images for + * horizontal and vertical derivatives. + * All edges of magnitude above t_high are always classified as edges. All edges + * below t_low are discarded. Edge values between t_low and t_high are classified + * as edges only if they are connected to edges having magnitude > t_high and are + * located in a direction perpendicular to that strong edge. + * + * \param[in] input_x Input point cloud passed by reference for the first derivative in the horizontal direction + * \param[in] input_y Input point cloud passed by reference for the first derivative in the vertical direction + * \param[out] output Output point cloud passed by reference + */ + void + canny (const pcl::PointCloud &input_x, + const pcl::PointCloud &input_y, + pcl::PointCloud &output); + + /** \brief This is a convenience function which performs edge detection based on + * the variable detector_kernel_type_ + * \param[out] output + */ + void + detectEdge (pcl::PointCloud &output); + + /** \brief All edges of magnitude above t_high are always classified as edges. + * All edges below t_low are discarded. + * Edge values between t_low and t_high are classified as edges only if they are + * connected to edges having magnitude > t_high and are located in a direction + * perpendicular to that strong edge. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgeCanny (pcl::PointCloud &output); + + /** \brief Uses the Sobel kernel for edge detection. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgeSobel (pcl::PointCloud &output); + + /** \brief Uses the Prewitt kernel for edge detection. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgePrewitt (pcl::PointCloud &output); + + /** \brief Uses the Roberts kernel for edge detection. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + detectEdgeRoberts (pcl::PointCloud &output); + + /** \brief Uses the LoG kernel for edge detection. + * Zero crossings of the Laplacian operator applied on an image indicate edges. + * Gaussian kernel is used to smoothen the image prior to the Laplacian. + * This is because Laplacian uses the second order derivative of the image and hence, is very sensitive to noise. + * The implementation is not two-step but rather applies the LoG kernel directly. + * + * \param[in] kernel_sigma variance of the LoG kernel used. + * \param[in] kernel_size a LoG kernel of dimensions kernel_size x kernel_size is used. + * \param[out] output Output point cloud passed by reference. + */ + void + detectEdgeLoG (const float kernel_sigma, const float kernel_size, + pcl::PointCloud &output); + + /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeYCentralKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeXCentral (pcl::PointCloud &output); + + /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYCentralKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeYCentral (pcl::PointCloud &output); + + /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeYForwardKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeXForward (pcl::PointCloud &output); + + /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYForwardKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeYForward (pcl::PointCloud &output); + + /** \brief Computes the image derivatives in X direction using the kernel kernel::derivativeXBackwardKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param output Output point cloud passed by reference + */ + void + computeDerivativeXBackward (pcl::PointCloud &output); + + /** \brief Computes the image derivatives in Y direction using the kernel kernel::derivativeYBackwardKernel. + * This function does NOT include a smoothing step. + * The image should be smoothed before using this function to reduce noise. + * \param[out] output Output point cloud passed by reference + */ + void + computeDerivativeYBackward (pcl::PointCloud &output); + + /** \brief Override function to implement the pcl::Filter interface + */ + void + applyFilter (pcl::PointCloud& /*output*/) {} + + /** \brief Set the input point cloud pointer + * \param[in] input pointer to input point cloud + */ + void + setInputCloud (PointCloudInPtr input) + { + input_ = input; + } + }; +} +#include "impl/edge.hpp" + +#endif // PCL_2D_EDGE_H + diff --git a/quasimodo/quasimodo_models/src/core/2d/impl/convolution.hpp b/quasimodo/quasimodo_models/src/core/2d/impl/convolution.hpp new file mode 100644 index 00000000..0ded38e8 --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/2d/impl/convolution.hpp @@ -0,0 +1,153 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_CONVOLUTION_IMPL_HPP +#define PCL_2D_CONVOLUTION_IMPL_HPP + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Convolution::filter (pcl::PointCloud &output) +{ + int input_row = 0; + int input_col = 0; + // default boundary option : zero padding + output = *input_; + + int iw = static_cast (input_->width), + ih = static_cast (input_->height), + kw = static_cast (kernel_.width), + kh = static_cast (kernel_.height); + switch (boundary_options_) + { + default: + case BOUNDARY_OPTION_CLAMP: + { + for (int i = 0; i < ih; i++) + { + for (int j = 0; j < iw; j++) + { + float intensity = 0; + for (int k = 0; k < kh; k++) + { + for (int l = 0; l < kw; l++) + { + int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2; + if (ikkh < 0) + input_row = 0; + else if (ikkh >= ih) + input_row = ih - 1; + else + input_row = ikkh; + + if (jlkw < 0) + input_col = 0; + else if (jlkw >= iw) + input_col = iw - 1; + else + input_col = jlkw; + + intensity += kernel_ (l, k).intensity * (*input_)(input_col, input_row).intensity; + } + } + output (j, i).intensity = intensity; + } + } + break; + } + + case BOUNDARY_OPTION_MIRROR: + { + for (int i = 0; i < ih; i++) + { + for (int j = 0; j < iw; j++) + { + float intensity = 0; + for (int k = 0; k < kh; k++) + { + for (int l = 0; l < kw; l++) + { + int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2; + if (ikkh < 0) + input_row = -ikkh - 1; + else if (ikkh >= ih) + input_row = 2 * ih - 1 - ikkh; + else + input_row = ikkh; + + if (jlkw < 0) + input_col = -jlkw - 1; + else if (jlkw >= iw) + input_col = 2 * iw - 1 - jlkw; + else + input_col = jlkw; + + intensity += kernel_ (l, k).intensity * ((*input_)(input_col, input_row).intensity); + } + } + output (j, i).intensity = intensity; + } + } + break; + } + + case BOUNDARY_OPTION_ZERO_PADDING: + { + for (int i = 0; i < ih; i++) + { + for (int j = 0; j < iw; j++) + { + float intensity = 0; + for (int k = 0; k < kh; k++) + { + for (int l = 0; l < kw; l++) + { + int ikkh = i + k - kh / 2, jlkw = j + l - kw / 2; + if (ikkh < 0 || ikkh >= ih || jlkw < 0 || jlkw >= iw) + continue; + else + intensity += kernel_ (l, k).intensity * ((*input_)(jlkw, ikkh).intensity); + } + } + output (j, i).intensity = intensity; + } + } + break; + } + } // switch +} + +#endif diff --git a/quasimodo/quasimodo_models/src/core/2d/impl/edge.hpp b/quasimodo/quasimodo_models/src/core/2d/impl/edge.hpp new file mode 100644 index 00000000..07e0afa3 --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/2d/impl/edge.hpp @@ -0,0 +1,494 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_EDGE_IMPL_HPP +#define PCL_2D_EDGE_IMPL_HPP + +#include "../convolution.h" +#include // rad2deg() +#include + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::detectEdgeSobel ( + pcl::PointCloud &output) +{ + //pcl::console::TicToc tt; + //tt.tic (); + convolution_.setInputCloud (input_); + pcl::PointCloud::Ptr kernel_x (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x (new pcl::PointCloud); + kernel_.setKernelType (kernel::SOBEL_X); + kernel_.fetchKernel (*kernel_x); + convolution_.setKernel (*kernel_x); + convolution_.filter (*magnitude_x); + //PCL_ERROR ("Convolve X: %g\n", tt.toc ()); tt.tic (); + + pcl::PointCloud::Ptr kernel_y (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y (new pcl::PointCloud); + kernel_.setKernelType (kernel::SOBEL_Y); + kernel_.fetchKernel (*kernel_y); + convolution_.setKernel (*kernel_y); + convolution_.filter (*magnitude_y); + //PCL_ERROR ("Convolve Y: %g\n", tt.toc ()); tt.tic (); + + const int height = input_->height; + const int width = input_->width; + + output.resize (height * width); + output.height = height; + output.width = width; + + for (size_t i = 0; i < output.size (); ++i) + { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } + //PCL_ERROR ("Rest: %g\n", tt.toc ()); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::sobelMagnitudeDirection ( + const pcl::PointCloud &input_x, + const pcl::PointCloud &input_y, + pcl::PointCloud &output) +{ + convolution_.setInputCloud (input_x.makeShared()); + pcl::PointCloud::Ptr kernel_x (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x (new pcl::PointCloud); + kernel_.setKernelType (kernel::SOBEL_X); + kernel_.fetchKernel (*kernel_x); + convolution_.setKernel (*kernel_x); + convolution_.filter (*magnitude_x); + + convolution_.setInputCloud (input_y.makeShared()); + pcl::PointCloud::Ptr kernel_y (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y (new pcl::PointCloud); + kernel_.setKernelType (kernel::SOBEL_Y); + kernel_.fetchKernel (*kernel_y); + convolution_.setKernel (*kernel_y); + convolution_.filter (*magnitude_y); + + const int height = input_x.height; + const int width = input_x.width; + + output.resize (height * width); + output.height = height; + output.width = width; + + for (size_t i = 0; i < output.size (); ++i) + { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::detectEdgePrewitt (pcl::PointCloud &output) +{ + convolution_.setInputCloud (input_); + + pcl::PointCloud::Ptr kernel_x (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x (new pcl::PointCloud); + kernel_.setKernelType (kernel::PREWITT_X); + kernel_.fetchKernel (*kernel_x); + convolution_.setKernel (*kernel_x); + convolution_.filter (*magnitude_x); + + pcl::PointCloud::Ptr kernel_y (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y (new pcl::PointCloud); + kernel_.setKernelType (kernel::PREWITT_Y); + kernel_.fetchKernel (*kernel_y); + convolution_.setKernel (*kernel_y); + convolution_.filter (*magnitude_y); + + const int height = input_->height; + const int width = input_->width; + + output.resize (height * width); + output.height = height; + output.width = width; + + for (size_t i = 0; i < output.size (); ++i) + { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::detectEdgeRoberts (pcl::PointCloud &output) +{ + convolution_.setInputCloud (input_); + + pcl::PointCloud::Ptr kernel_x (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_x (new pcl::PointCloud); + kernel_.setKernelType (kernel::ROBERTS_X); + kernel_.fetchKernel (*kernel_x); + convolution_.setKernel (*kernel_x); + convolution_.filter (*magnitude_x); + + pcl::PointCloud::Ptr kernel_y (new pcl::PointCloud); + pcl::PointCloud::Ptr magnitude_y (new pcl::PointCloud); + kernel_.setKernelType (kernel::ROBERTS_Y); + kernel_.fetchKernel (*kernel_y); + convolution_.setKernel (*kernel_y); + convolution_.filter (*magnitude_y); + + const int height = input_->height; + const int width = input_->width; + + output.resize (height * width); + output.height = height; + output.width = width; + + for (size_t i = 0; i < output.size (); ++i) + { + output[i].magnitude_x = (*magnitude_x)[i].intensity; + output[i].magnitude_y = (*magnitude_y)[i].intensity; + output[i].magnitude = + sqrtf ((*magnitude_x)[i].intensity * (*magnitude_x)[i].intensity + + (*magnitude_y)[i].intensity * (*magnitude_y)[i].intensity); + output[i].direction = + atan2f ((*magnitude_y)[i].intensity, (*magnitude_x)[i].intensity); + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::cannyTraceEdge ( + int rowOffset, int colOffset, int row, int col, + pcl::PointCloud &maxima) +{ + int newRow = row + rowOffset; + int newCol = col + colOffset; + PointXYZI &pt = maxima (newCol, newRow); + + if (newRow > 0 && newRow < static_cast (maxima.height) && newCol > 0 && newCol < static_cast (maxima.width)) + { + if (pt.intensity == 0.0f || pt.intensity == std::numeric_limits::max ()) + return; + + pt.intensity = std::numeric_limits::max (); + cannyTraceEdge ( 1, 0, newRow, newCol, maxima); + cannyTraceEdge (-1, 0, newRow, newCol, maxima); + cannyTraceEdge ( 1, 1, newRow, newCol, maxima); + cannyTraceEdge (-1, -1, newRow, newCol, maxima); + cannyTraceEdge ( 0, -1, newRow, newCol, maxima); + cannyTraceEdge ( 0, 1, newRow, newCol, maxima); + cannyTraceEdge (-1, 1, newRow, newCol, maxima); + cannyTraceEdge ( 1, -1, newRow, newCol, maxima); + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::discretizeAngles (pcl::PointCloud &thet) +{ + const int height = thet.height; + const int width = thet.width; + float angle; + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + angle = pcl::rad2deg (thet (j, i).direction); + if (((angle <= 22.5) && (angle >= -22.5)) || (angle >= 157.5) || (angle <= -157.5)) + thet (j, i).direction = 0; + else + if (((angle > 22.5) && (angle < 67.5)) || ((angle < -112.5) && (angle > -157.5))) + thet (j, i).direction = 45; + else + if (((angle >= 67.5) && (angle <= 112.5)) || ((angle <= -67.5) && (angle >= -112.5))) + thet (j, i).direction = 90; + else + if (((angle > 112.5) && (angle < 157.5)) || ((angle < -22.5) && (angle > -67.5))) + thet (j, i).direction = 135; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::suppressNonMaxima ( + const pcl::PointCloud &edges, + pcl::PointCloud &maxima, float tLow) +{ + const int height = edges.height; + const int width = edges.width; + + maxima.height = height; + maxima.width = width; + maxima.resize (height * width); + + for (size_t i = 0; i < maxima.size (); ++i) + maxima[i].intensity = 0.0f; + + // tHigh and non-maximal supression + for (int i = 1; i < height - 1; i++) + { + for (int j = 1; j < width - 1; j++) + { + const PointXYZIEdge &ptedge = edges (j, i); + PointXYZI &ptmax = maxima (j, i); + + if (ptedge.magnitude < tLow) + continue; + + //maxima (j, i).intensity = 0; + + switch (int (ptedge.direction)) + { + case 0: + { + if (ptedge.magnitude >= edges (j - 1, i).magnitude && + ptedge.magnitude >= edges (j + 1, i).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + case 45: + { + if (ptedge.magnitude >= edges (j - 1, i - 1).magnitude && + ptedge.magnitude >= edges (j + 1, i + 1).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + case 90: + { + if (ptedge.magnitude >= edges (j, i - 1).magnitude && + ptedge.magnitude >= edges (j, i + 1).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + case 135: + { + if (ptedge.magnitude >= edges (j + 1, i - 1).magnitude && + ptedge.magnitude >= edges (j - 1, i + 1).magnitude) + ptmax.intensity = ptedge.magnitude; + break; + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::detectEdgeCanny (pcl::PointCloud &output) +{ + float tHigh = hysteresis_threshold_high_; + float tLow = hysteresis_threshold_low_; + const int height = input_->height; + const int width = input_->width; + + output.resize (height * width); + output.height = height; + output.width = width; + + //pcl::console::TicToc tt; + //tt.tic (); + + // Noise reduction using gaussian blurring + pcl::PointCloud::Ptr gaussian_kernel (new pcl::PointCloud); + PointCloudInPtr smoothed_cloud (new PointCloudIn); + kernel_.setKernelSize (3); + kernel_.setKernelSigma (1.0); + kernel_.setKernelType (kernel::GAUSSIAN); + kernel_.fetchKernel (*gaussian_kernel); + convolution_.setKernel (*gaussian_kernel); + convolution_.setInputCloud (input_); + convolution_.filter (*smoothed_cloud); + //PCL_ERROR ("Gaussian blur: %g\n", tt.toc ()); tt.tic (); + + // Edge detection usign Sobel + pcl::PointCloud::Ptr edges (new pcl::PointCloud); + setInputCloud (smoothed_cloud); + detectEdgeSobel (*edges); + //PCL_ERROR ("Sobel: %g\n", tt.toc ()); tt.tic (); + + // Edge discretization + discretizeAngles (*edges); + //PCL_ERROR ("Discretize: %g\n", tt.toc ()); tt.tic (); + + // tHigh and non-maximal supression + pcl::PointCloud::Ptr maxima (new pcl::PointCloud); + suppressNonMaxima (*edges, *maxima, tLow); + //PCL_ERROR ("NM suppress: %g\n", tt.toc ()); tt.tic (); + + // Edge tracing + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits::max ()) + continue; + + (*maxima)(j, i).intensity = std::numeric_limits::max (); + cannyTraceEdge ( 1, 0, i, j, *maxima); + cannyTraceEdge (-1, 0, i, j, *maxima); + cannyTraceEdge ( 1, 1, i, j, *maxima); + cannyTraceEdge (-1, -1, i, j, *maxima); + cannyTraceEdge ( 0, -1, i, j, *maxima); + cannyTraceEdge ( 0, 1, i, j, *maxima); + cannyTraceEdge (-1, 1, i, j, *maxima); + cannyTraceEdge ( 1, -1, i, j, *maxima); + } + } + //PCL_ERROR ("Edge tracing: %g\n", tt.toc ()); + + // Final thresholding + for (size_t i = 0; i < input_->size (); ++i) + { + if ((*maxima)[i].intensity == std::numeric_limits::max ()) + output[i].magnitude = 255; + else + output[i].magnitude = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::canny ( + const pcl::PointCloud &input_x, + const pcl::PointCloud &input_y, + pcl::PointCloud &output) +{ + float tHigh = hysteresis_threshold_high_; + float tLow = hysteresis_threshold_low_; + const int height = input_x.height; + const int width = input_x.width; + + output.resize (height * width); + output.height = height; + output.width = width; + + // Noise reduction using gaussian blurring + pcl::PointCloud::Ptr gaussian_kernel (new pcl::PointCloud); + kernel_.setKernelSize (3); + kernel_.setKernelSigma (1.0); + kernel_.setKernelType (kernel::GAUSSIAN); + kernel_.fetchKernel (*gaussian_kernel); + convolution_.setKernel (*gaussian_kernel); + + PointCloudIn smoothed_cloud_x; + convolution_.setInputCloud (input_x.makeShared()); + convolution_.filter (smoothed_cloud_x); + + PointCloudIn smoothed_cloud_y; + convolution_.setInputCloud (input_y.makeShared()); + convolution_.filter (smoothed_cloud_y); + + + // Edge detection usign Sobel + pcl::PointCloud::Ptr edges (new pcl::PointCloud); + sobelMagnitudeDirection (smoothed_cloud_x, smoothed_cloud_y, *edges.get ()); + + // Edge discretization + discretizeAngles (*edges); + + pcl::PointCloud::Ptr maxima (new pcl::PointCloud); + suppressNonMaxima (*edges, *maxima, tLow); + + // Edge tracing + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + if ((*maxima)(j, i).intensity < tHigh || (*maxima)(j, i).intensity == std::numeric_limits::max ()) + continue; + + (*maxima)(j, i).intensity = std::numeric_limits::max (); + cannyTraceEdge ( 1, 0, i, j, *maxima); + cannyTraceEdge (-1, 0, i, j, *maxima); + cannyTraceEdge ( 1, 1, i, j, *maxima); + cannyTraceEdge (-1, -1, i, j, *maxima); + cannyTraceEdge ( 0, -1, i, j, *maxima); + cannyTraceEdge ( 0, 1, i, j, *maxima); + cannyTraceEdge (-1, 1, i, j, *maxima); + cannyTraceEdge ( 1, -1, i, j, *maxima); + } + } + + // Final thresholding + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + if ((*maxima)(j, i).intensity == std::numeric_limits::max ()) + output (j, i).magnitude = 255; + else + output (j, i).magnitude = 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Edge::detectEdgeLoG ( + const float kernel_sigma, const float kernel_size, + pcl::PointCloud &output) +{ + convolution_.setInputCloud (input_); + + pcl::PointCloud::Ptr log_kernel (new pcl::PointCloud); + kernel_.setKernelType (kernel::LOG); + kernel_.setKernelSigma (kernel_sigma); + kernel_.setKernelSize (kernel_size); + kernel_.fetchKernel (*log_kernel); + convolution_.setKernel (*log_kernel); + convolution_.filter (output); +} + +#endif diff --git a/quasimodo/quasimodo_models/src/core/2d/impl/kernel.hpp b/quasimodo/quasimodo_models/src/core/2d/impl/kernel.hpp new file mode 100644 index 00000000..223fba8c --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/2d/impl/kernel.hpp @@ -0,0 +1,328 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_KERNEL_IMPL_HPP +#define PCL_2D_KERNEL_IMPL_HPP + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::fetchKernel (pcl::PointCloud &kernel) +{ + switch (kernel_type_) + { + case SOBEL_X: + { + sobelKernelX (kernel); + break; + } + case SOBEL_Y: + { + sobelKernelY (kernel); + break; + } + case PREWITT_X: + { + prewittKernelX (kernel); + break; + } + case PREWITT_Y: + { + prewittKernelY (kernel); + break; + } + case ROBERTS_X: + { + robertsKernelX (kernel); + break; + } + case ROBERTS_Y: + { + robertsKernelY (kernel); + break; + } + case LOG: + { + loGKernel (kernel); + break; + } + case DERIVATIVE_CENTRAL_X: + { + derivativeXCentralKernel (kernel); + break; + } + case DERIVATIVE_FORWARD_X: + { + derivativeXForwardKernel (kernel); + break; + } + case DERIVATIVE_BACKWARD_X: + { + derivativeXBackwardKernel (kernel); + break; + } + case DERIVATIVE_CENTRAL_Y: + { + derivativeYCentralKernel (kernel); + break; + } + case DERIVATIVE_FORWARD_Y: + { + derivativeYForwardKernel (kernel); + break; + } + case DERIVATIVE_BACKWARD_Y: + { + derivativeYBackwardKernel (kernel); + break; + } + case GAUSSIAN: + { + gaussianKernel (kernel); + break; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::gaussianKernel (pcl::PointCloud &kernel) +{ + float sum = 0; + kernel.resize (kernel_size_ * kernel_size_); + kernel.height = kernel_size_; + kernel.width = kernel_size_; + + double sigma_sqr = 2 * sigma_ * sigma_; + + for (int i = 0; i < kernel_size_; i++) + { + for (int j = 0; j < kernel_size_; j++) + { + int iks = (i - kernel_size_ / 2); + int jks = (j - kernel_size_ / 2); + kernel (j, i).intensity = expf (float (- double (iks * iks + jks * jks) / sigma_sqr)); + sum += float (kernel (j, i).intensity); + } + } + + // Normalizing the kernel + for (size_t i = 0; i < kernel.size (); ++i) + kernel[i].intensity /= sum; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::loGKernel (pcl::PointCloud &kernel) +{ + float sum = 0; + float temp = 0; + kernel.resize (kernel_size_ * kernel_size_); + kernel.height = kernel_size_; + kernel.width = kernel_size_; + + double sigma_sqr = 2 * sigma_ * sigma_; + + for (int i = 0; i < kernel_size_; i++) + { + for (int j = 0; j < kernel_size_; j++) + { + int iks = (i - kernel_size_ / 2); + int jks = (j - kernel_size_ / 2); + temp = float (double (iks * iks + jks * jks) / sigma_sqr); + kernel (j, i).intensity = (1.0f - temp) * expf (-temp); + sum += kernel (j, i).intensity; + } + } + + // Normalizing the kernel + for (size_t i = 0; i < kernel.size (); ++i) + kernel[i].intensity /= sum; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::sobelKernelX (pcl::PointCloud &kernel) +{ + kernel.resize (9); + kernel.height = 3; + kernel.width = 3; + kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1; + kernel (0, 1).intensity = -2; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 2; + kernel (0, 2).intensity = -1; kernel (1, 2).intensity = 0; kernel (2, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::prewittKernelX (pcl::PointCloud &kernel) +{ + kernel.resize (9); + kernel.height = 3; + kernel.width = 3; + kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1; + kernel (0, 1).intensity = -1; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 1; + kernel (0, 2).intensity = -1; kernel (1, 2).intensity = 0; kernel (2, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::robertsKernelX (pcl::PointCloud &kernel) +{ + kernel.resize (4); + kernel.height = 2; + kernel.width = 2; + kernel (0, 0).intensity = 1; kernel (1, 0).intensity = 0; + kernel (0, 1).intensity = 0; kernel (1, 1).intensity = -1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::sobelKernelY (pcl::PointCloud &kernel) +{ + kernel.resize (9); + kernel.height = 3; + kernel.width = 3; + kernel (0, 0).intensity = -1; kernel (1, 0).intensity = -2; kernel (2, 0).intensity = -1; + kernel (0, 1).intensity = 0; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 0; + kernel (0, 2).intensity = 1; kernel (1, 2).intensity = 2; kernel (2, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::prewittKernelY (pcl::PointCloud &kernel) +{ + kernel.resize (9); + kernel.height = 3; + kernel.width = 3; + kernel (0, 0).intensity = 1; kernel (1, 0).intensity = 1; kernel (2, 0).intensity = 1; + kernel (0, 1).intensity = 0; kernel (1, 1).intensity = 0; kernel (2, 1).intensity = 0; + kernel (0, 2).intensity = -1; kernel (1, 2).intensity = -1; kernel (2, 2).intensity = -1; +} + +template void +pcl::kernel::robertsKernelY (pcl::PointCloud &kernel) +{ + kernel.resize (4); + kernel.height = 2; + kernel.width = 2; + kernel (0, 0).intensity = 0; kernel (1, 0).intensity = 1; + kernel (0, 1).intensity = -1; kernel (1, 1).intensity = 0; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeXCentralKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 1; + kernel.width = 3; + kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 0; kernel (2, 0).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeXForwardKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 1; + kernel.width = 3; + kernel (0, 0).intensity = 0; kernel (1, 0).intensity = -1; kernel (2, 0).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeXBackwardKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 1; + kernel.width = 3; + kernel (0, 0).intensity = -1; kernel (1, 0).intensity = 1; kernel (2, 0).intensity = 0; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeYCentralKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 3; + kernel.width = 1; + kernel (0, 0).intensity = -1; kernel (0, 1).intensity = 0; kernel (0, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeYForwardKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 3; + kernel.width = 1; + kernel (0, 0).intensity = 0; kernel (0, 1).intensity = -1; kernel (0, 2).intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::derivativeYBackwardKernel (pcl::PointCloud &kernel) +{ + kernel.resize (3); + kernel.height = 3; + kernel.width = 1; + kernel (0, 0).intensity = -1; kernel (0, 1).intensity = 1; kernel (0, 2).intensity = 0; +} + +////////////////////////////////////////////////////////////////////////////// +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::setKernelType (KERNEL_ENUM kernel_type) +{ + kernel_type_ = kernel_type; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::setKernelSize (int kernel_size) +{ + kernel_size_ = kernel_size; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::kernel::setKernelSigma (float kernel_sigma) +{ + sigma_ = kernel_sigma; +} + + +#endif diff --git a/quasimodo/quasimodo_models/src/core/2d/impl/keypoint.hpp b/quasimodo/quasimodo_models/src/core/2d/impl/keypoint.hpp new file mode 100644 index 00000000..f0417dc3 --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/2d/impl/keypoint.hpp @@ -0,0 +1,247 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * keypoint.hpp + * + * Created on: May 28, 2012 + * Author: somani + */ + +#ifndef PCL_2D_KEYPOINT_HPP_ +#define PCL_2D_KEYPOINT_HPP_ + +#include "../edge.h" +#include "../convolution.h" +#include + +////////////////////////////////////////////////////////////////////////////// +void +pcl::keypoint::harrisCorner (ImageType &output, ImageType &input, const float sigma_d, const float sigma_i, const float alpha, const float thresh){ + + /*creating the gaussian kernels*/ + ImageType kernel_d; + ImageType kernel_i; + conv_2d.gaussianKernel (5, sigma_d, kernel_d); + conv_2d.gaussianKernel (5, sigma_i, kernel_i); + + /*scaling the image with differentiation scale*/ + ImageType smoothed_image; + conv_2d.convolve (smoothed_image, kernel_d, input); + + /*image derivatives*/ + ImageType I_x, I_y; + edge_detection.ComputeDerivativeXCentral (I_x, smoothed_image); + edge_detection.ComputeDerivativeYCentral (I_y, smoothed_image); + + /*second moment matrix*/ + ImageType I_x2, I_y2, I_xI_y; + imageElementMultiply (I_x2, I_x, I_x); + imageElementMultiply (I_y2, I_y, I_y); + imageElementMultiply (I_xI_y, I_x, I_y); + + /*scaling second moment matrix with integration scale*/ + ImageType M00, M10, M11; + conv_2d.convolve (M00, kernel_i, I_x2); + conv_2d.convolve (M10, kernel_i, I_xI_y); + conv_2d.convolve (M11, kernel_i, I_y2); + + /*harris function*/ + const size_t height = input.size (); + const size_t width = input[0].size (); + output.resize (height); + for (size_t i = 0; i < height; i++) + { + output[i].resize (width); + for (size_t j = 0; j < width; j++) + { + output[i][j] = M00[i][j] * M11[i][j] - (M10[i][j] * M10[i][j]) - alpha * ((M00[i][j] + M11[i][j]) * (M00[i][j] + M11[i][j])); + if (thresh != 0) + { + if (output[i][j] < thresh) + output[i][j] = 0; + else + output[i][j] = 255; + } + } + } + + /*local maxima*/ + for (size_t i = 1; i < height - 1; i++) + { + for (size_t j = 1; j < width - 1; j++) + { + if (output[i][j] > output[i - 1][j - 1] && output[i][j] > output[i - 1][j] && output[i][j] > output[i - 1][j + 1] && + output[i][j] > output[i][j - 1] && output[i][j] > output[i][j + 1] && + output[i][j] > output[i + 1][j - 1] && output[i][j] > output[i + 1][j] && output[i][j] > output[i + 1][j + 1]) + ; + else + output[i][j] = 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +void +pcl::keypoint::hessianBlob (ImageType &output, ImageType &input, const float sigma, bool SCALED){ + /*creating the gaussian kernels*/ + ImageType kernel, cornerness; + conv_2d.gaussianKernel (5, sigma, kernel); + + /*scaling the image with differentiation scale*/ + ImageType smoothed_image; + conv_2d.convolve (smoothed_image, kernel, input); + + /*image derivatives*/ + ImageType I_x, I_y; + edge_detection.ComputeDerivativeXCentral (I_x, smoothed_image); + edge_detection.ComputeDerivativeYCentral (I_y, smoothed_image); + + /*second moment matrix*/ + ImageType I_xx, I_yy, I_xy; + edge_detection.ComputeDerivativeXCentral (I_xx, I_x); + edge_detection.ComputeDerivativeYCentral (I_xy, I_x); + edge_detection.ComputeDerivativeYCentral (I_yy, I_y); + /*Determinant of Hessian*/ + const size_t height = input.size (); + const size_t width = input[0].size (); + float min = std::numeric_limits::max(); + float max = std::numeric_limits::min(); + cornerness.resize (height); + for (size_t i = 0; i < height; i++) + { + cornerness[i].resize (width); + for (size_t j = 0; j < width; j++) + { + cornerness[i][j] = sigma*sigma*(I_xx[i][j]+I_yy[i][j]-I_xy[i][j]*I_xy[i][j]); + if(SCALED){ + if(cornerness[i][j] < min) + min = cornerness[i][j]; + if(cornerness[i][j] > max) + max = cornerness[i][j]; + } + } + + /*local maxima*/ + output.resize (height); + output[0].resize (width); + output[height-1].resize (width); + for (size_t i = 1; i < height - 1; i++) + { + output[i].resize (width); + for (size_t j = 1; j < width - 1; j++) + { + if(SCALED) + output[i][j] = ((cornerness[i][j]-min)/(max-min)); + else + output[i][j] = cornerness[i][j]; + } + } + } +} + +////////////////////////////////////////////////////////////////////////////// +void +pcl::keypoint::hessianBlob (ImageType &output, ImageType &input, const float start_scale, const float scaling_factor, const int num_scales){ + const size_t height = input.size(); + const size_t width = input[0].size(); + const int local_search_radius = 1; + float scale = start_scale; + std::vector cornerness; + cornerness.resize(num_scales); + for(int i = 0;i < num_scales;i++){ + hessianBlob(cornerness[i], input, scale, false); + scale *= scaling_factor; + } + bool non_max_flag = false; + float scale_max, local_max; + for(size_t i = 0;i < height;i++){ + for(size_t j = 0;j < width;j++){ + scale_max = std::numeric_limits::min(); + /*default output in case of no blob at the current point is 0*/ + output[i][j] = 0; + for(int k = 0;k < num_scales;k++){ + /*check if the current point (k,i,j) is a maximum in the defined search radius*/ + non_max_flag = false; + local_max = cornerness[k][i][j]; + for(int n = -local_search_radius; n <= local_search_radius;n++){ + if(n+k < 0 || n+k >= num_scales) + continue; + for(int l = -local_search_radius;l <= local_search_radius;l++){ + if(l+i < 0 || l+i >= height) + continue; + for(int m = -local_search_radius; m <= local_search_radius;m++){ + if(m+j < 0 || m+j >= width) + continue; + if(cornerness[n+k][l+i][m+j] > local_max){ + non_max_flag = true; + break; + } + } + if(non_max_flag) + break; + } + if(non_max_flag) + break; + } + /*if the current point is a point of local maximum, check if it is a maximum point across scales*/ + if(!non_max_flag){ + if(cornerness[k][i][j] > scale_max){ + scale_max = cornerness[k][i][j]; + /*output indicates the scale at which the blob is found at the current location in the image*/ + output[i][i] = start_scale*pow(scaling_factor, k); + } + } + } + } + } +} + +////////////////////////////////////////////////////////////////////////////// +void +pcl::keypoint::imageElementMultiply (ImageType &output, ImageType &input1, ImageType &input2){ + const size_t height = input1.size (); + const size_t width = input1[0].size (); + output.resize (height); + for (size_t i = 0; i < height; i++) + { + output[i].resize (width); + for (size_t j = 0; j < width; j++) + { + output[i][j] = input1[i][j] * input2[i][j]; + } + } +} + +#endif // PCL_2D_KEYPOINT_HPP_ diff --git a/quasimodo/quasimodo_models/src/core/2d/impl/morphology.hpp b/quasimodo/quasimodo_models/src/core/2d/impl/morphology.hpp new file mode 100644 index 00000000..6528eed9 --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/2d/impl/morphology.hpp @@ -0,0 +1,380 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_MORPHOLOGY_HPP_ +#define PCL_2D_MORPHOLOGY_HPP_ + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template void +pcl::Morphology::erosionBinary (pcl::PointCloud &output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + bool mismatch_flag; + + output.width = width; + output.height = height; + output.resize (width * height); + + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + // Operation done only at 1's + if ((*input_)(j, i).intensity == 0) + { + output (j, i).intensity = 0; + continue; + } + mismatch_flag = false; + for (int k = 0; k < kernel_height; k++) + { + if (mismatch_flag) + break; + for (int l = 0; l < kernel_width; l++) + { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width) + { + continue; + } + // If one of the elements of the kernel and image dont match, + // the output image is 0. So, move to the next point. + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity != 1) + { + output (j, i).intensity = 0; + mismatch_flag = true; + break; + } + } + } + // Assign value according to mismatch flag + output (j, i).intensity = (mismatch_flag) ? 0 : 1; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template void +pcl::Morphology::dilationBinary (pcl::PointCloud &output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + bool match_flag; + + output.width = width; + output.height = height; + output.resize (width * height); + + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + match_flag = false; + for (int k = 0; k < kernel_height; k++) + { + if (match_flag) + break; + for (int l = 0; l < kernel_width; l++) + { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= height) + { + continue; + } + // If any position where kernel is 1 and image is also one is detected, + // matching occurs + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity == 1) + { + match_flag = true; + break; + } + } + } + // Assign value according to match flag + output (j, i).intensity = (match_flag) ? 1 : 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template void +pcl::Morphology::openingBinary (pcl::PointCloud &output) +{ + PointCloudInPtr intermediate_output (new PointCloudIn); + erosionBinary (*intermediate_output); + this->setInputCloud (intermediate_output); + dilationBinary (output); +} + +////////////////////////////////////////////////////////////////////////////// +// Assumes input, kernel and output images have 0's and 1's only +template void +pcl::Morphology::closingBinary (pcl::PointCloud &output) +{ + PointCloudInPtr intermediate_output (new PointCloudIn); + dilationBinary (*intermediate_output); + this->setInputCloud (intermediate_output); + erosionBinary (output); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::erosionGray (pcl::PointCloud &output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + float min; + output.resize (width * height); + output.width = width; + output.height = height; + + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + min = -1; + for (int k = 0; k < kernel_height; k++) + { + for (int l = 0; l < kernel_width; l++) + { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width) + { + continue; + } + // If one of the elements of the kernel and image dont match, + // the output image is 0. So, move to the next point. + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity < min || min == -1) + { + min = (*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity; + } + } + } + // Assign value according to mismatch flag + output (j, i).intensity = min; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::dilationGray (pcl::PointCloud &output) +{ + const int height = input_->height; + const int width = input_->width; + const int kernel_height = structuring_element_->height; + const int kernel_width = structuring_element_->width; + float max; + + output.resize (width * height); + output.width = width; + output.height = height; + + for (int i = 0; i < height; i++) + { + for (int j = 0; j < width; j++) + { + max = -1; + for (int k = 0; k < kernel_height; k++) + { + for (int l = 0; l < kernel_width; l++) + { + // We only check for 1's in the kernel + if ((*structuring_element_)(l, k).intensity == 0) + continue; + if ((i + k - kernel_height / 2) < 0 || (i + k - kernel_height / 2) >= height || (j + l - kernel_width / 2) < 0 || (j + l - kernel_width / 2) >= width) + { + continue; + } + // If one of the elements of the kernel and image dont match, + // the output image is 0. So, move to the next point. + if ((*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity > max || max == -1) + { + max = (*input_)(j + l - kernel_width / 2, i + k - kernel_height / 2).intensity; + } + } + } + // Assign value according to mismatch flag + output (j, i).intensity = max; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::openingGray (pcl::PointCloud &output) +{ + PointCloudInPtr intermediate_output (new PointCloudIn); + erosionGray (*intermediate_output); + this->setInputCloud (intermediate_output); + dilationGray (output); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::closingGray (pcl::PointCloud &output) +{ + PointCloudInPtr intermediate_output (new PointCloudIn); + dilationGray (*intermediate_output); + this->setInputCloud (intermediate_output); + erosionGray (output); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::subtractionBinary ( + pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2) +{ + const int height = (input1.height < input2.hieght) ? input1.height : input2.height; + const int width = (input1.width < input2.width) ? input1.width : input2.width; + output.width = width; + output.height = height; + output.resize (height * width); + + for (size_t i = 0; i < output.size (); ++i) + { + if (input1[i].intensity == 1 && input2[i].intensity == 0) + output[i].intensity = 1; + else + output[i].intensity = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::unionBinary ( + pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2) +{ + const int height = (input1.height < input2.hieght) ? input1.height : input2.height; + const int width = (input1.width < input2.width) ? input1.width : input2.width; + output.width = width; + output.height = height; + output.resize (height * width); + + for (size_t i = 0; i < output.size (); ++i) + { + if (input1[i].intensity == 1 || input2[i].intensity == 1) + output[i].intensity = 1; + else + output[i].intensity = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::intersectionBinary ( + pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2) +{ + const int height = (input1.height < input2.height) ? input1.height : input2.height; + const int width = (input1.width < input2.width) ? input1.width : input2.width; + output.width = width; + output.height = height; + output.resize (height * width); + + for (size_t i = 0; i < output.size (); ++i) + { + if (input1[i].intensity == 1 && input2[i].intensity == 1) + output[i].intensity = 1; + else + output[i].intensity = 0; + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::structuringElementCircular ( + pcl::PointCloud &kernel, const int radius) +{ + const int dim = 2 * radius; + kernel.height = dim; + kernel.width = dim; + kernel.resize (dim * dim); + + for (int i = 0; i < dim; i++) + { + for (int j = 0; j < dim; j++) + { + if (((i - radius) * (i - radius) + (j - radius) * (j - radius)) < radius * radius) + kernel (j, i).intensity = 1; + else + kernel (j, i).intensity = 0; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::structuringElementRectangle ( + pcl::PointCloud &kernel, const int height, const int width) +{ + kernel.height = height; + kernel.width = width; + kernel.resize (height * width); + for (size_t i = 0; i < kernel.size (); ++i) + kernel[i].intensity = 1; +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::Morphology::setStructuringElement (const PointCloudInPtr &structuring_element) +{ + structuring_element_ = structuring_element; +} + +#endif // PCL_2D_MORPHOLOGY_HPP_ diff --git a/quasimodo/quasimodo_models/src/core/2d/kernel.h b/quasimodo/quasimodo_models/src/core/2d/kernel.h new file mode 100644 index 00000000..1bf85e22 --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/2d/kernel.h @@ -0,0 +1,244 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_KERNEL_H_ +#define PCL_2D_KERNEL_H_ +#include +#include +namespace pcl +{ + template + class kernel + { + public: + + /** + * enumerates the different types of kernels available. + */ + enum KERNEL_ENUM + { + SOBEL_X, //!< SOBEL_X + SOBEL_Y, //!< SOBEL_Y + PREWITT_X, //!< PREWITT_X + PREWITT_Y, //!< PREWITT_Y + ROBERTS_X, //!< ROBERTS_X + ROBERTS_Y, //!< ROBERTS_Y + LOG, //!< LOG + DERIVATIVE_CENTRAL_X, //!< DERIVATIVE_CENTRAL_X + DERIVATIVE_FORWARD_X, //!< DERIVATIVE_FORWARD_X + DERIVATIVE_BACKWARD_X,//!< DERIVATIVE_BACKWARD_X + DERIVATIVE_CENTRAL_Y, //!< DERIVATIVE_CENTRAL_Y + DERIVATIVE_FORWARD_Y, //!< DERIVATIVE_FORWARD_Y + DERIVATIVE_BACKWARD_Y,//!< DERIVATIVE_BACKWARD_Y + GAUSSIAN //!< GAUSSIAN + }; + + int kernel_size_; + float sigma_; + KERNEL_ENUM kernel_type_; + + kernel () : + kernel_size_ (3), + sigma_ (1.0), + kernel_type_ (GAUSSIAN) + { + + } + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * Helper function which returns the kernel selected by the kernel_type_ enum + */ + void fetchKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * Gaussian kernel with size (kernel_size_ x kernel_size_) and variance sigma_ + */ + + void gaussianKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * Laplacian of Gaussian kernel with size (kernel_size_ x kernel_size_) and variance sigma_ + */ + + void loGKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Sobel kernel in the X direction + */ + + void sobelKernelX (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Prewitt kernel in the X direction + */ + + void prewittKernelX (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 2x2 Roberts kernel in the X direction + */ + + void robertsKernelX (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Sobel kernel in the Y direction + */ + + void sobelKernelY (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 3x3 Prewitt kernel in the Y direction + */ + + void prewittKernelY (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * 2x2 Roberts kernel in the Y direction + */ + + void robertsKernelY (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 0 1] + */ + + void derivativeXCentralKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 0 1]' + */ + + void derivativeYCentralKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [0 -1 1] + */ + + void derivativeXForwardKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [0 -1 1]' + */ + + void derivativeYForwardKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 1 0] + */ + + void derivativeXBackwardKernel (pcl::PointCloud &kernel); + + /** + * + * @param kernel Kernel point cloud passed by reference + * + * kernel [-1 1 0]' + */ + + void derivativeYBackwardKernel (PointCloud &kernel); + + /** + * + * @param kernel_type enum indicating the kernel type wanted + * + * select the kernel type. + */ + void setKernelType (KERNEL_ENUM kernel_type); + + /** + * + * @param kernel_size kernel of size kernel_size x kernel_size is created(LoG and Gaussian only) + * + * Setter function for kernel_size_ + */ + void setKernelSize (int kernel_size); + + /** + * + * @param kernel_sigma variance of the Gaussian or LoG kernels. + * + * Setter function for kernel_sigma_ + */ + void setKernelSigma (float kernel_sigma); + }; +} + +#include "impl/kernel.hpp" + +#endif // PCL_2D_KERNEL_H_ diff --git a/quasimodo/quasimodo_models/src/core/2d/keypoint.h b/quasimodo/quasimodo_models/src/core/2d/keypoint.h new file mode 100644 index 00000000..7e6f6381 --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/2d/keypoint.h @@ -0,0 +1,75 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * keypoint.h + * + * Created on: May 28, 2012 + * Author: somani + */ + +#ifndef PCL_2D_KEYPOINT_H_ +#define PCL_2D_KEYPOINT_H_ + +#include "edge.h" + +namespace pcl +{ + class Keypoint + { + private: + Edge edge_detection; + Convolution conv_2d; + public: + Keypoint () + { + } + + void + harrisCorner (ImageType &output, ImageType &input, const float sigma_d, const float sigma_i, const float alpha, const float thresh); + + void + hessianBlob (ImageType &output, ImageType &input, const float sigma, bool SCALE); + + void + hessianBlob (ImageType &output, ImageType &input, const float start_scale, const float scaling_factor, const int num_scales); + + void + imageElementMultiply (ImageType &output, ImageType &input1, ImageType &input2); + }; +} + +#include "impl/keypoint.hpp" + +#endif // PCL_2D_KEYPOINT_H_ diff --git a/quasimodo/quasimodo_models/src/core/2d/morphology.h b/quasimodo/quasimodo_models/src/core/2d/morphology.h new file mode 100644 index 00000000..d96618fb --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/2d/morphology.h @@ -0,0 +1,200 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_2D_MORPHOLOGY_H_ +#define PCL_2D_MORPHOLOGY_H_ + +#include + +namespace pcl +{ + template + class Morphology : public PCLBase + { + private: + typedef typename pcl::PointCloud PointCloudIn; + typedef typename PointCloudIn::Ptr PointCloudInPtr; + + PointCloudInPtr structuring_element_; + + public: + using PCLBase::input_; + + Morphology () {} + + /** \brief This function performs erosion followed by dilation. + * It is useful for removing noise in the form of small blobs and patches. + * \param[out] output Output point cloud passed by reference + */ + void + openingBinary (pcl::PointCloud &output); + + /** \brief This function performs dilation followed by erosion. + * It is useful for filling up (holes/cracks/small discontinuities) in a binary + * segmented region + * \param[out] output Output point cloud passed by reference + */ + void + closingBinary (pcl::PointCloud &output); + + /** \brief Binary dilation is similar to a logical disjunction of sets. + * At each pixel having value 1, if for all pixels in the structuring element having value 1, the corresponding + * pixels in the input image are also 1, the center pixel is set to 1. Otherwise, it is set to 0. + * \param[out] output Output point cloud passed by reference + */ + void + erosionBinary (pcl::PointCloud &output); + + /** \brief Binary erosion is similar to a logical addition of sets. + * At each pixel having value 1, if at least one pixel in the structuring element is 1 and the corresponding point + * in the input image is 1, the center pixel is set to 1. Otherwise, it is set to 0. + * \param[out] output Output point cloud passed by reference + */ + void + dilationBinary (pcl::PointCloud &output); + + /** \brief Grayscale erosion followed by dilation. + * This is used to remove small bright artifacts from the image. Large bright objects are relatively undisturbed. + * \param[out] output Output point cloud passed by reference + */ + void + openingGray (pcl::PointCloud &output); + + /** \brief Grayscale dilation followed by erosion. + * This is used to remove small dark artifacts from the image. Bright features or large dark features are relatively undisturbed. + * \param[out] output Output point cloud passed by reference + */ + void + closingGray (pcl::PointCloud &output); + + /** \brief Takes the min of the pixels where kernel is 1 + * \param[out] output Output point cloud passed by reference + */ + void + erosionGray (pcl::PointCloud &output); + + /** \brief Takes the max of the pixels where kernel is 1 + * \param[out] output Output point cloud passed by reference + */ + void + dilationGray (pcl::PointCloud &output); + + /** \brief Set operation + * output = input1 - input2 + * \param[out] output Output point cloud passed by reference + * \param[in] input1 + * \param[in] input2 + */ + void + subtractionBinary (pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2); + + /** \brief Set operation + * \f$output = input1 \cup input2\f$ + * \param[out] output Output point cloud passed by reference + * \param[in] input1 + * \param[in] input2 + */ + void + unionBinary (pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2); + + /** \brief Set operation \f$ output = input1 \cap input2 \f$ + * \param[out] output Output point cloud passed by reference + * \param[in] input1 + * \param[in] input2 + */ + void + intersectionBinary (pcl::PointCloud &output, + const pcl::PointCloud &input1, + const pcl::PointCloud &input2); + + /** \brief Creates a circular structing element. The size of the kernel created is 2*radius x 2*radius. + * Center of the structuring element is the center of the circle. + * All values lying on the circle are 1 and the others are 0. + * + * \param[out] kernel structuring element kernel passed by reference + * \param[in] radius Radius of the circular structuring element. + */ + void + structuringElementCircular (pcl::PointCloud &kernel, const int radius); + + /** \brief Creates a rectangular structing element of size height x width. * + * All values are 1. + * + * \param[out] kernel structuring element kernel passed by reference + * \param[in] height height number of rows in the structuring element + * \param[in] width number of columns in the structuring element + * + */ + void + structuringElementRectangle (pcl::PointCloud &kernel, + const int height, const int width); + + enum MORPHOLOGICAL_OPERATOR_TYPE + { + EROSION_GRAY, + DILATION_GRAY, + OPENING_GRAY, + CLOSING_GRAY, + EROSION_BINARY, + DILATION_BINARY, + OPENING_BINARY, + CLOSING_BINARY + }; + + MORPHOLOGICAL_OPERATOR_TYPE operator_type; + + /** + * \param[out] output Output point cloud passed by reference + */ + void + applyMorphologicalOperation (pcl::PointCloud &output); + + /** + * \param[in] structuring_element The structuring element to be used for the morphological operation + */ + void + setStructuringElement (const PointCloudInPtr &structuring_element); + }; +} + +#include "impl/morphology.hpp" + +#endif // PCL_2D_MORPHOLOGY_H_ diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index cfa495c5..2ee5705c 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -29,6 +29,17 @@ #include +#include +#include +#include +#include "organized_edge_detection.hpp" +#include +#include +#include +#include +#include +#include + namespace reglib { @@ -37,14 +48,14 @@ RGBDFrame::RGBDFrame(){ id = RGBDFrame_id_counter++; capturetime = 0; pose = Eigen::Matrix4d::Identity(); + keyval = ""; } bool updated = true; void on_trackbar( int, void* ){updated = true;} RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals){ - - //printf("%s LINE:%i\n",__FILE__,__LINE__); + keyval = ""; sweepid = -1; id = RGBDFrame_id_counter++; @@ -68,6 +79,11 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy; + // +// printf("camera: cx %5.5f cy %5.5f",cx,cy); +// printf(": fx %5.5f fy %5.5f",camera->fx,camera->fy); +// printf(": ifx %5.5f ify %5.5f\n",ifx,ify); + connections.resize(1); connections[0].resize(1); connections[0][0] = 0; @@ -87,7 +103,10 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt depthedges.create(height,width,CV_8UC1); unsigned char * depthedgesdata = (unsigned char *)depthedges.data; - + for(int i = 0; i < width*height; i++){ + depthedgesdata[i] = 0; + } +/* double t = 0.01; for(int w = 0; w < width; w++){ for(int h = 0; h < height;h++){ @@ -151,22 +170,26 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } } - +*/ //printf("%s LINE:%i\n",__FILE__,__LINE__); if(compute_normals){ normals.create(height,width,CV_32FC3); float * normalsdata = (float *)normals.data; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr normals (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr normals_cloud (new pcl::PointCloud); cloud->width = width; cloud->height = height; cloud->points.resize(width*height); + //cloud->dense = false; for(int w = 0; w < width; w++){ for(int h = 0; h < height;h++){ int ind = h*width+w; - pcl::PointXYZ & p = cloud->points[ind]; + pcl::PointXYZRGBA & p = cloud->points[ind]; + p.b = rgbdata[3*ind+0]; + p.g = rgbdata[3*ind+1]; + p.r = rgbdata[3*ind+2]; double z = idepth*double(depthdata[ind]); if(z > 0){ p.x = (double(w) - cx) * z * ifx; @@ -181,7 +204,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } //printf("%s LINE:%i\n",__FILE__,__LINE__); - pcl::IntegralImageNormalEstimation ne; + pcl::IntegralImageNormalEstimation ne; ne.setInputCloud(cloud); bool tune = false; @@ -196,12 +219,17 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); ne.setNormalSmoothingSize(NormalSmoothingSize); ne.setDepthDependentSmoothing (depth_dependent_smoothing); - ne.compute(*normals); + ne.compute(*normals_cloud); for(int w = 0; w < width; w++){ for(int h = 0; h < height;h++){ int ind = h*width+w; - pcl::Normal p2 = normals->points[ind]; + pcl::PointXYZRGBA p = cloud->points[ind]; + pcl::Normal p2 = normals_cloud->points[ind]; + + + //if(w % 20 == 0 && h % 20 == 0){printf("%i %i -> point(%f %f %f) normal(%f %f %f)\n",w,h,p.x,p.y,p.z,p2.normal_x,p2.normal_y,p2.normal_z);} + if(!isnan(p2.normal_x)){ normalsdata[3*ind+0] = p2.normal_x; normalsdata[3*ind+1] = p2.normal_y; @@ -214,6 +242,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } + //printf("%s LINE:%i\n",__FILE__,__LINE__); if(tune){ combined.create(height,2*width,CV_8UC3); @@ -237,11 +266,11 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); ne.setNormalSmoothingSize(NormalSmoothingSize); ne.setDepthDependentSmoothing (depth_dependent_smoothing); - ne.compute(*normals); + ne.compute(*normals_cloud); for(int w = 0; w < width; w++){ for(int h = 0; h < height;h++){ int ind = h*width+w; - pcl::Normal p2 = normals->points[ind]; + pcl::Normal p2 = normals_cloud->points[ind]; if(!isnan(p2.normal_x)){ normalsdata[3*ind+0] = p2.normal_x; normalsdata[3*ind+1] = p2.normal_y; @@ -263,7 +292,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt combidata[3*indc+0] = rgbdata[3*ind+0]; combidata[3*indc+1] = rgbdata[3*ind+1]; combidata[3*indc+2] = rgbdata[3*ind+2]; - pcl::Normal p2 = normals->points[ind]; + pcl::Normal p2 = normals_cloud->points[ind]; if(!isnan(p2.normal_x)){ combidata[3*indn+0] = 255.0*fabs(p2.normal_x); combidata[3*indn+1] = 255.0*fabs(p2.normal_y); @@ -286,11 +315,63 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt //} } - //printf("%s LINE:%i\n",__FILE__,__LINE__); - } - //show(true); + //pcl::PointCloud::Ptr cloud2 = getPCLcloud(); + pcl::OrganizedEdgeFromRGBNormals oed; + oed.setInputNormals (normals_cloud); + oed.setInputCloud (cloud); + oed.setDepthDisconThreshold (0.02); // 2cm +// oed.setHCCannyLowThreshold (0.4); +// oed.setHCCannyHighThreshold (1.1); + oed.setMaxSearchNeighbors (50); + pcl::PointCloud labels; + std::vector label_indices; + oed.compute (labels, label_indices); + +// pcl::PointCloud::Ptr occluding_edges (new pcl::PointCloud), +// occluded_edges (new pcl::PointCloud), +// boundary_edges (new pcl::PointCloud), +// high_curvature_edges (new pcl::PointCloud), +// rgb_edges (new pcl::PointCloud); + +// pcl::PointCloud::Ptr cloud3 (new pcl::PointCloud); +// pcl::copyPointCloud (*cloud2, label_indices[0].indices, *boundary_edges); +// pcl::copyPointCloud (*cloud2, label_indices[1].indices, *occluding_edges); +// pcl::copyPointCloud (*cloud2, label_indices[2].indices, *occluded_edges); +// pcl::copyPointCloud (*cloud2, label_indices[3].indices, *high_curvature_edges); +// pcl::copyPointCloud (*cloud2, label_indices[4].indices, *rgb_edges); + +// cv::Mat out = rgb.clone(); + + for(unsigned int i = 0; i < label_indices[1].indices.size(); i++){ + int ind = label_indices[1].indices[i]; + depthedgesdata[ind] = 255; +// out.data[3*ind+0] =255; +// out.data[3*ind+1] =0; +// out.data[3*ind+2] =255; + } - //printf("%s LINE:%i\n",__FILE__,__LINE__); + for(unsigned int i = 0; i < label_indices[3].indices.size(); i++){ + int ind = label_indices[3].indices[i]; + depthedgesdata[ind] = 255; +// out.data[3*ind+0] =0; +// out.data[3*ind+1] =255; +// out.data[3*ind+2] =255; + } + + + for(unsigned int i = 0; i < label_indices[4].indices.size(); i++){ + int ind = label_indices[4].indices[i]; + //depthedgesdata[ind] = 255; +// out.data[3*ind+0] =0; +// out.data[3*ind+1] =255; +// out.data[3*ind+2] =0; + } + +// cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", out ); +// cv::waitKey(0); + + //show(true); + } } RGBDFrame::~RGBDFrame(){} @@ -322,16 +403,21 @@ pcl::PointCloud::Ptr RGBDFrame::getPCLcloud(){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; double z = idepth*double(depthdata[ind]); + + pcl::PointXYZRGB p; + p.b = rgbdata[3*ind+0]; + p.g = rgbdata[3*ind+1]; + p.r = rgbdata[3*ind+2]; if(z > 0){ - pcl::PointXYZRGB p; p.x = (double(w) - cx) * z * ifx; p.y = (double(h) - cy) * z * ify; p.z = z; - p.b = rgbdata[3*ind+0]; - p.g = rgbdata[3*ind+1]; - p.r = rgbdata[3*ind+2]; - cloud->points[ind] = p; + }else{ + p.x = NAN; + p.y = NAN; + p.z = NAN; } + cloud->points[ind] = p; } } diff --git a/quasimodo/quasimodo_models/src/core/Util.cpp b/quasimodo/quasimodo_models/src/core/Util.cpp index 5fe0d122..b7253caa 100644 --- a/quasimodo/quasimodo_models/src/core/Util.cpp +++ b/quasimodo/quasimodo_models/src/core/Util.cpp @@ -162,7 +162,7 @@ namespace reglib{ Xn = transformation*Xn; } - bool isconverged(std::vector before, std::vector after, double stopvalr, double stopvalt){ + bool isconverged(std::vector before, std::vector after, double stopvalr, double stopvalt, bool verbose){ double change_trans = 0; double change_rot = 0; unsigned int nr_frames = after.size(); @@ -186,6 +186,10 @@ namespace reglib{ change_trans /= double(nr_frames*(nr_frames-1)); change_rot /= double(nr_frames*(nr_frames-1)); + if(verbose){ + printf("change_trans: %10.10f change_rot: %10.10f\n",change_trans,change_rot); + } + if(change_trans < stopvalt && change_rot < stopvalr){return true;} else{return false;} } diff --git a/quasimodo/quasimodo_models/src/core/organized_edge_detection.h b/quasimodo/quasimodo_models/src/core/organized_edge_detection.h new file mode 100644 index 00000000..41b54cda --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/organized_edge_detection.h @@ -0,0 +1,431 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FEATURES_ORGANIZED_EDGE_DETECTION_H_ +#define PCL_FEATURES_ORGANIZED_EDGE_DETECTION_H_ + +#include +#include + +namespace pcl +{ + /** \brief OrganizedEdgeBase, OrganizedEdgeFromRGB, OrganizedEdgeFromNormals, + * and OrganizedEdgeFromRGBNormals find 3D edges from an organized point + * cloud data. Given an organized point cloud, they will output a PointCloud + * of edge labels and a vector of PointIndices. + * OrganizedEdgeBase accepts PCL_XYZ_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, and EDGELABEL_OCCLUDED. + * OrganizedEdgeFromRGB accepts PCL_RGB_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, and EDGELABEL_RGB_CANNY. + * OrganizedEdgeFromNormals accepts PCL_XYZ_POINT_TYPES with PCL_NORMAL_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, and EDGELABEL_HIGH_CURVATURE. + * OrganizedEdgeFromRGBNormals accepts PCL_RGB_POINT_TYPES with PCL_NORMAL_POINT_TYPES and returns EDGELABEL_NAN_BOUNDARY, EDGELABEL_OCCLUDING, EDGELABEL_OCCLUDED, EDGELABEL_HIGH_CURVATURE, and EDGELABEL_RGB_CANNY. + * + * \author Changhyun Choi + */ + template + class OrganizedEdgeBase : public PCLBase + { + typedef typename pcl::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + + typedef typename pcl::PointCloud PointCloudL; + typedef typename PointCloudL::Ptr PointCloudLPtr; + typedef typename PointCloudL::ConstPtr PointCloudLConstPtr; + + public: + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + using PCLBase::input_; + using PCLBase::indices_; + using PCLBase::initCompute; + using PCLBase::deinitCompute; + + /** \brief Constructor for OrganizedEdgeBase */ + OrganizedEdgeBase () + : th_depth_discon_ (0.02f) + , max_search_neighbors_ (50) + , detecting_edge_types_ (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_HIGH_CURVATURE) + { + } + + /** \brief Destructor for OrganizedEdgeBase */ + virtual + ~OrganizedEdgeBase () + { + } + + /** \brief Perform the 3D edge detection (edges from depth discontinuities) + * \param[out] labels a PointCloud of edge labels + * \param[out] label_indices a vector of PointIndices corresponding to each edge label + */ + void + compute (pcl::PointCloud& labels, std::vector& label_indices) const; + + /** \brief Set the tolerance in meters for difference in depth values between neighboring points. */ + inline void + setDepthDisconThreshold (const float th) + { + th_depth_discon_ = th; + } + + /** \brief Get the tolerance in meters for difference in depth values between neighboring points. */ + inline float + getDepthDisconThreshold () const + { + return (th_depth_discon_); + } + + /** \brief Set the max search distance for deciding occluding and occluded edges. */ + inline void + setMaxSearchNeighbors (const int max_dist) + { + max_search_neighbors_ = max_dist; + } + + /** \brief Get the max search distance for deciding occluding and occluded edges. */ + inline int + getMaxSearchNeighbors () const + { + return (max_search_neighbors_); + } + + /** \brief Set the detecting edge types. */ + inline void + setEdgeType (int edge_types) + { + detecting_edge_types_ = edge_types; + } + + /** \brief Get the detecting edge types. */ + inline int + getEdgeType () const + { + return detecting_edge_types_; + } + + enum {EDGELABEL_NAN_BOUNDARY=1, EDGELABEL_OCCLUDING=2, EDGELABEL_OCCLUDED=4, EDGELABEL_HIGH_CURVATURE=8, EDGELABEL_RGB_CANNY=16}; + static const int num_of_edgetype_ = 5; + + protected: + /** \brief Perform the 3D edge detection (edges from depth discontinuities) and assign point indices for each edge label + * \param[out] labels a PointCloud of edge labels + */ + void + extractEdges (pcl::PointCloud& labels) const; + + /** \brief Assign point indices for each edge label + * \param[out] labels a PointCloud of edge labels + * \param[out] label_indices a vector of PointIndices corresponding to each edge label + */ + void + assignLabelIndices (pcl::PointCloud& labels, std::vector& label_indices) const; + + struct Neighbor + { + Neighbor (int dx, int dy, int didx) + : d_x (dx) + , d_y (dy) + , d_index (didx) + {} + + int d_x; + int d_y; + int d_index; // = dy * width + dx: pre-calculated + }; + + /** \brief The tolerance in meters for difference in depth values between neighboring points + * (The value is set for 1 meter and is adapted with respect to depth value linearly. + * (e.g. 2.0*th_depth_discon_ in 2 meter depth)) + */ + float th_depth_discon_; + + /** \brief The max search distance for deciding occluding and occluded edges */ + int max_search_neighbors_; + + /** \brief The bit encoded value that represents edge types to detect */ + int detecting_edge_types_; + }; + + template + class OrganizedEdgeFromRGB : virtual public OrganizedEdgeBase + { + typedef typename pcl::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + + typedef typename pcl::PointCloud PointCloudL; + typedef typename PointCloudL::Ptr PointCloudLPtr; + typedef typename PointCloudL::ConstPtr PointCloudLConstPtr; + + public: + using OrganizedEdgeBase::input_; + using OrganizedEdgeBase::indices_; + using OrganizedEdgeBase::initCompute; + using OrganizedEdgeBase::deinitCompute; + using OrganizedEdgeBase::detecting_edge_types_; + using OrganizedEdgeBase::EDGELABEL_NAN_BOUNDARY; + using OrganizedEdgeBase::EDGELABEL_OCCLUDING; + using OrganizedEdgeBase::EDGELABEL_OCCLUDED; + using OrganizedEdgeBase::EDGELABEL_RGB_CANNY; + + /** \brief Constructor for OrganizedEdgeFromRGB */ + OrganizedEdgeFromRGB () + : OrganizedEdgeBase () + , th_rgb_canny_low_ (40.0) + , th_rgb_canny_high_ (100.0) + { + this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_RGB_CANNY); + } + + /** \brief Destructor for OrganizedEdgeFromRGB */ + virtual + ~OrganizedEdgeFromRGB () + { + } + + /** \brief Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) and assign point indices for each edge label + * \param[out] labels a PointCloud of edge labels + * \param[out] label_indices a vector of PointIndices corresponding to each edge label + */ + void + compute (pcl::PointCloud& labels, std::vector& label_indices) const; + + /** \brief Set the low threshold value for RGB Canny edge detection */ + inline void + setRGBCannyLowThreshold (const float th) + { + th_rgb_canny_low_ = th; + } + + /** \brief Get the low threshold value for RGB Canny edge detection */ + inline float + getRGBCannyLowThreshold () const + { + return (th_rgb_canny_low_); + } + + /** \brief Set the high threshold value for RGB Canny edge detection */ + inline void + setRGBCannyHighThreshold (const float th) + { + th_rgb_canny_high_ = th; + } + + /** \brief Get the high threshold value for RGB Canny edge detection */ + inline float + getRGBCannyHighThreshold () const + { + return (th_rgb_canny_high_); + } + + protected: + /** \brief Perform the 3D edge detection (edges from depth discontinuities and RGB Canny edge) + * \param[out] labels a PointCloud of edge labels + */ + void + extractEdges (pcl::PointCloud& labels) const; + + /** \brief The low threshold value for RGB Canny edge detection (default: 40.0) */ + float th_rgb_canny_low_; + + /** \brief The high threshold value for RGB Canny edge detection (default: 100.0) */ + float th_rgb_canny_high_; + }; + + template + class OrganizedEdgeFromNormals : virtual public OrganizedEdgeBase + { + typedef typename pcl::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + + typedef typename pcl::PointCloud PointCloudN; + typedef typename PointCloudN::Ptr PointCloudNPtr; + typedef typename PointCloudN::ConstPtr PointCloudNConstPtr; + + typedef typename pcl::PointCloud PointCloudL; + typedef typename PointCloudL::Ptr PointCloudLPtr; + typedef typename PointCloudL::ConstPtr PointCloudLConstPtr; + + public: + using OrganizedEdgeBase::input_; + using OrganizedEdgeBase::indices_; + using OrganizedEdgeBase::initCompute; + using OrganizedEdgeBase::deinitCompute; + using OrganizedEdgeBase::detecting_edge_types_; + using OrganizedEdgeBase::EDGELABEL_NAN_BOUNDARY; + using OrganizedEdgeBase::EDGELABEL_OCCLUDING; + using OrganizedEdgeBase::EDGELABEL_OCCLUDED; + using OrganizedEdgeBase::EDGELABEL_HIGH_CURVATURE; + + /** \brief Constructor for OrganizedEdgeFromNormals */ + OrganizedEdgeFromNormals () + : OrganizedEdgeBase () + , normals_ () + , th_hc_canny_low_ (0.4f) + , th_hc_canny_high_ (1.1f) + { + this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_HIGH_CURVATURE); + } + + /** \brief Destructor for OrganizedEdgeFromNormals */ + virtual + ~OrganizedEdgeFromNormals () + { + } + + /** \brief Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) and assign point indices for each edge label + * \param[out] labels a PointCloud of edge labels + * \param[out] label_indices a vector of PointIndices corresponding to each edge label + */ + void + compute (pcl::PointCloud& labels, std::vector& label_indices) const; + + /** \brief Provide a pointer to the input normals. + * \param[in] normals the input normal cloud + */ + inline void + setInputNormals (const PointCloudNConstPtr &normals) + { + normals_ = normals; + } + + /** \brief Get the input normals. */ + inline PointCloudNConstPtr + getInputNormals () const + { + return (normals_); + } + + /** \brief Set the low threshold value for high curvature Canny edge detection */ + inline void + setHCCannyLowThreshold (const float th) + { + th_hc_canny_low_ = th; + } + + /** \brief Get the low threshold value for high curvature Canny edge detection */ + inline float + getHCCannyLowThreshold () const + { + return (th_hc_canny_low_); + } + + /** \brief Set the high threshold value for high curvature Canny edge detection */ + inline void + setHCCannyHighThreshold (const float th) + { + th_hc_canny_high_ = th; + } + + /** \brief Get the high threshold value for high curvature Canny edge detection */ + inline float + getHCCannyHighThreshold () const + { + return (th_hc_canny_high_); + } + + protected: + /** \brief Perform the 3D edge detection (edges from depth discontinuities and high curvature regions) + * \param[out] labels a PointCloud of edge labels + */ + void + extractEdges (pcl::PointCloud& labels) const; + + /** \brief A pointer to the input normals */ + PointCloudNConstPtr normals_; + + /** \brief The low threshold value for high curvature Canny edge detection (default: 0.4) */ + float th_hc_canny_low_; + + /** \brief The high threshold value for high curvature Canny edge detection (default: 1.1) */ + float th_hc_canny_high_; + }; + + template + class OrganizedEdgeFromRGBNormals : public OrganizedEdgeFromRGB, public OrganizedEdgeFromNormals + { + typedef typename pcl::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + typedef typename PointCloud::ConstPtr PointCloudConstPtr; + + typedef typename pcl::PointCloud PointCloudN; + typedef typename PointCloudN::Ptr PointCloudNPtr; + typedef typename PointCloudN::ConstPtr PointCloudNConstPtr; + + typedef typename pcl::PointCloud PointCloudL; + typedef typename PointCloudL::Ptr PointCloudLPtr; + typedef typename PointCloudL::ConstPtr PointCloudLConstPtr; + + public: + using OrganizedEdgeFromNormals::input_; + using OrganizedEdgeFromNormals::indices_; + using OrganizedEdgeFromNormals::initCompute; + using OrganizedEdgeFromNormals::deinitCompute; + using OrganizedEdgeFromNormals::detecting_edge_types_; + using OrganizedEdgeBase::EDGELABEL_NAN_BOUNDARY; + using OrganizedEdgeBase::EDGELABEL_OCCLUDING; + using OrganizedEdgeBase::EDGELABEL_OCCLUDED; + using OrganizedEdgeBase::EDGELABEL_HIGH_CURVATURE; + using OrganizedEdgeBase::EDGELABEL_RGB_CANNY; + + /** \brief Constructor for OrganizedEdgeFromRGBNormals */ + OrganizedEdgeFromRGBNormals () + : OrganizedEdgeFromRGB () + , OrganizedEdgeFromNormals () + { + this->setEdgeType (EDGELABEL_NAN_BOUNDARY | EDGELABEL_OCCLUDING | EDGELABEL_OCCLUDED | EDGELABEL_RGB_CANNY | EDGELABEL_HIGH_CURVATURE); + } + + /** \brief Destructor for OrganizedEdgeFromRGBNormals */ + virtual + ~OrganizedEdgeFromRGBNormals () + { + } + + /** \brief Perform the 3D edge detection (edges from depth discontinuities, RGB Canny edge, and high curvature regions) and assign point indices for each edge label + * \param[out] labels a PointCloud of edge labels + * \param[out] label_indices a vector of PointIndices corresponding to each edge label + */ + void + compute (pcl::PointCloud& labels, std::vector& label_indices) const; + }; +} + + +#include "organized_edge_detection.hpp" + +#endif //#ifndef PCL_FEATURES_ORGANIZED_EDGE_DETECTION_H_ diff --git a/quasimodo/quasimodo_models/src/core/organized_edge_detection.hpp b/quasimodo/quasimodo_models/src/core/organized_edge_detection.hpp new file mode 100644 index 00000000..e3f2f95c --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/organized_edge_detection.hpp @@ -0,0 +1,354 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2012-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_ +#define PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_ + +#include "2d/edge.h" +#include "organized_edge_detection.h" +#include +#include + +/** + * Directions: 1 2 3 + * 0 x 4 + * 7 6 5 + * e.g. direction y means we came from pixel with label y to the center pixel x + */ +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeBase::compute (pcl::PointCloud& labels, std::vector& label_indices) const +{ + pcl::Label invalid_pt; + invalid_pt.label = unsigned (0); + labels.points.resize (input_->points.size (), invalid_pt); + labels.width = input_->width; + labels.height = input_->height; + + extractEdges (labels); + + assignLabelIndices (labels, label_indices); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeBase::assignLabelIndices (pcl::PointCloud& labels, std::vector& label_indices) const +{ + const unsigned invalid_label = unsigned (0); + label_indices.resize (num_of_edgetype_); + for (unsigned idx = 0; idx < input_->points.size (); idx++) + { + if (labels[idx].label != invalid_label) + { + for (int edge_type = 0; edge_type < num_of_edgetype_; edge_type++) + { + if ((labels[idx].label >> edge_type) & 1) + label_indices[edge_type].indices.push_back (idx); + } + } + } +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeBase::extractEdges (pcl::PointCloud& labels) const +{ + if ((detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) || (detecting_edge_types_ & EDGELABEL_OCCLUDING) || (detecting_edge_types_ & EDGELABEL_OCCLUDED)) + { + // Fill lookup table for next points to visit + const int num_of_ngbr = 8; + Neighbor directions [num_of_ngbr] = {Neighbor(-1, 0, -1), + Neighbor(-1, -1, -labels.width - 1), + Neighbor( 0, -1, -labels.width ), + Neighbor( 1, -1, -labels.width + 1), + Neighbor( 1, 0, 1), + Neighbor( 1, 1, labels.width + 1), + Neighbor( 0, 1, labels.width ), + Neighbor(-1, 1, labels.width - 1)}; + + for (int row = 1; row < int(input_->height) - 1; row++) + { + for (int col = 1; col < int(input_->width) - 1; col++) + { + int curr_idx = row*int(input_->width) + col; + if (!pcl_isfinite (input_->points[curr_idx].z)) + continue; + + float curr_depth = fabsf (input_->points[curr_idx].z); + + // Calculate depth distances between current point and neighboring points + std::vector nghr_dist; + nghr_dist.resize (8); + bool found_invalid_neighbor = false; + for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++) + { + int nghr_idx = curr_idx + directions[d_idx].d_index; + assert (nghr_idx >= 0 && nghr_idx < input_->points.size ()); + if (!pcl_isfinite (input_->points[nghr_idx].z)) + { + found_invalid_neighbor = true; + break; + } + nghr_dist[d_idx] = curr_depth - fabsf (input_->points[nghr_idx].z); + } + + if (!found_invalid_neighbor) + { + // Every neighboring points are valid + std::vector::iterator min_itr = std::min_element (nghr_dist.begin (), nghr_dist.end ()); + std::vector::iterator max_itr = std::max_element (nghr_dist.begin (), nghr_dist.end ()); + float nghr_dist_min = *min_itr; + float nghr_dist_max = *max_itr; + float dist_dominant = fabsf (nghr_dist_min) > fabsf (nghr_dist_max) ? nghr_dist_min : nghr_dist_max; + if (fabsf (dist_dominant) > th_depth_discon_*fabsf (curr_depth*curr_depth)) + { + // Found a depth discontinuity + if (dist_dominant > 0.f) + { + if (detecting_edge_types_ & EDGELABEL_OCCLUDED) + labels[curr_idx].label |= EDGELABEL_OCCLUDED; + } + else + { + if (detecting_edge_types_ & EDGELABEL_OCCLUDING) + labels[curr_idx].label |= EDGELABEL_OCCLUDING; + } + } + } + else + { + // Some neighboring points are not valid (nan points) + // Search for corresponding point across invalid points + // Search direction is determined by nan point locations with respect to current point + int dx = 0; + int dy = 0; + int num_of_invalid_pt = 0; + for (int d_idx = 0; d_idx < num_of_ngbr; d_idx++) + { + int nghr_idx = curr_idx + directions[d_idx].d_index; + assert (nghr_idx >= 0 && nghr_idx < input_->points.size ()); + if (!pcl_isfinite (input_->points[nghr_idx].z)) + { + dx += directions[d_idx].d_x; + dy += directions[d_idx].d_y; + num_of_invalid_pt++; + } + } + + // Search directions + assert (num_of_invalid_pt > 0); + float f_dx = static_cast (dx) / static_cast (num_of_invalid_pt); + float f_dy = static_cast (dy) / static_cast (num_of_invalid_pt); + + // Search for corresponding point across invalid points + float corr_depth = std::numeric_limits::quiet_NaN (); + for (int s_idx = 1; s_idx < max_search_neighbors_; s_idx++) + { + int s_row = row + static_cast (std::floor (f_dy*static_cast (s_idx))); + int s_col = col + static_cast (std::floor (f_dx*static_cast (s_idx))); + + if (s_row < 0 || s_row >= int(input_->height) || s_col < 0 || s_col >= int(input_->width)) + break; + + if (pcl_isfinite (input_->points[s_row*int(input_->width)+s_col].z)) + { + corr_depth = fabsf (input_->points[s_row*int(input_->width)+s_col].z); + break; + } + } + + if (!pcl_isnan (corr_depth)) + { + // Found a corresponding point + float dist = curr_depth - corr_depth; + if (fabsf (dist) > th_depth_discon_*fabsf (curr_depth)) + { + // Found a depth discontinuity + if (dist > 0.f) + { + if (detecting_edge_types_ & EDGELABEL_OCCLUDED) + labels[curr_idx].label |= EDGELABEL_OCCLUDED; + } + else + { + if (detecting_edge_types_ & EDGELABEL_OCCLUDING) + labels[curr_idx].label |= EDGELABEL_OCCLUDING; + } + } + } + else + { + // Not found a corresponding point, just nan boundary edge + if (detecting_edge_types_ & EDGELABEL_NAN_BOUNDARY) + labels[curr_idx].label |= EDGELABEL_NAN_BOUNDARY; + } + } + } + } + } +} + + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeFromRGB::compute (pcl::PointCloud& labels, std::vector& label_indices) const +{ + pcl::Label invalid_pt; + invalid_pt.label = unsigned (0); + labels.points.resize (input_->points.size (), invalid_pt); + labels.width = input_->width; + labels.height = input_->height; + + OrganizedEdgeBase::extractEdges (labels); + extractEdges (labels); + + this->assignLabelIndices (labels, label_indices); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeFromRGB::extractEdges (pcl::PointCloud& labels) const +{ + if ((detecting_edge_types_ & EDGELABEL_RGB_CANNY)) + { + pcl::PointCloud::Ptr gray (new pcl::PointCloud); + gray->width = input_->width; + gray->height = input_->height; + gray->resize (input_->height*input_->width); + + for (size_t i = 0; i < input_->size (); ++i) + (*gray)[i].intensity = float (((*input_)[i].r + (*input_)[i].g + (*input_)[i].b) / 3); + + pcl::PointCloud img_edge_rgb; + pcl::Edge edge; + edge.setInputCloud (gray); + edge.setHysteresisThresholdLow (th_rgb_canny_low_); + edge.setHysteresisThresholdHigh (th_rgb_canny_high_); + edge.detectEdgeCanny (img_edge_rgb); + + for (uint32_t row=0; row void +pcl::OrganizedEdgeFromNormals::compute (pcl::PointCloud& labels, std::vector& label_indices) const +{ + pcl::Label invalid_pt; + invalid_pt.label = unsigned (0); + labels.points.resize (input_->points.size (), invalid_pt); + labels.width = input_->width; + labels.height = input_->height; + + OrganizedEdgeBase::extractEdges (labels); + extractEdges (labels); + + this->assignLabelIndices (labels, label_indices); +} + +////////////////////////////////////////////////////////////////////////////// +template void +pcl::OrganizedEdgeFromNormals::extractEdges (pcl::PointCloud& labels) const +{ + if ((detecting_edge_types_ & EDGELABEL_HIGH_CURVATURE)) + { + + pcl::PointCloud nx, ny; + nx.width = normals_->width; + nx.height = normals_->height; + nx.resize (normals_->height*normals_->width); + + ny.width = normals_->width; + ny.height = normals_->height; + ny.resize (normals_->height*normals_->width); + + for (uint32_t row=0; rowheight; row++) + { + for (uint32_t col=0; colwidth; col++) + { + nx (col, row).intensity = normals_->points[row*normals_->width + col].normal_x; + ny (col, row).intensity = normals_->points[row*normals_->width + col].normal_y; + } + } + + pcl::PointCloud img_edge; + pcl::Edge edge; + edge.setHysteresisThresholdLow (th_hc_canny_low_); + edge.setHysteresisThresholdHigh (th_hc_canny_high_); + edge.canny (nx, ny, img_edge); + + for (uint32_t row=0; row void +pcl::OrganizedEdgeFromRGBNormals::compute (pcl::PointCloud& labels, std::vector& label_indices) const +{ + pcl::Label invalid_pt; + invalid_pt.label = unsigned (0); + labels.points.resize (input_->points.size (), invalid_pt); + labels.width = input_->width; + labels.height = input_->height; + + OrganizedEdgeBase::extractEdges (labels); + OrganizedEdgeFromNormals::extractEdges (labels); + OrganizedEdgeFromRGB::extractEdges (labels); + + this->assignLabelIndices (labels, label_indices); +} + +#define PCL_INSTANTIATE_OrganizedEdgeBase(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeBase; +#define PCL_INSTANTIATE_OrganizedEdgeFromRGB(T,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGB; +#define PCL_INSTANTIATE_OrganizedEdgeFromNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromNormals; +#define PCL_INSTANTIATE_OrganizedEdgeFromRGBNormals(T,NT,LT) template class PCL_EXPORTS pcl::OrganizedEdgeFromRGBNormals; + +#endif //#ifndef PCL_FEATURES_IMPL_ORGANIZED_EDGE_DETECTION_H_ diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 7a88f823..77fab06a 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -31,15 +31,276 @@ Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){ recomputeModelPoints(); } -void Model::recomputeModelPoints(){ +void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask){ + bool * maskvec = modelmask->maskvec; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + unsigned int frameid = frame->id; + + Matrix4d ip = p.inverse(); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + float im00 = ip(0,0); float im01 = ip(0,1); float im02 = ip(0,2); float im03 = ip(0,3); + float im10 = ip(1,0); float im11 = ip(1,1); float im12 = ip(1,2); float im13 = ip(1,3); + float im20 = ip(2,0); float im21 = ip(2,1); float im22 = ip(2,2); float im23 = ip(2,3); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + const unsigned int dst_width2 = camera->width - 2; + const unsigned int dst_height2 = camera->height - 2; + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float fx = camera->fx; + const float fy = camera->fy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + bool * isfused = new bool[width*height]; + for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} + + + for(unsigned int ind = 0; ind < spvec.size();ind++){ + superpoint & sp = spvec[ind]; + + float src_x = sp.point(0); + float src_y = sp.point(1); + float src_z = sp.point(2); + + float src_nx = sp.normal(0); + float src_ny = sp.normal(1); + float src_nz = sp.normal(2); + + float src_r = sp.feature(0); + float src_g = sp.feature(1); + float src_b = sp.feature(2); + + double point_information = sp.point_information; + double feature_information = sp.feature_information; + + float tx = im00*src_x + im01*src_y + im02*src_z + im03; + float ty = im10*src_x + im11*src_y + im12*src_z + im13; + float tz = im20*src_x + im21*src_y + im22*src_z + im23; + + float itz = 1.0/tz; + float dst_w = fx*tx*itz + cx; + float dst_h = fy*ty*itz + cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * width + unsigned(dst_w+0.5); + + float dst_z = idepth*float(depthdata[dst_ind]); + float dst_nx = normalsdata[3*dst_ind+0]; + if(!isfused[dst_ind] && dst_z > 0 && dst_nx != 2){ + float dst_ny = normalsdata[3*dst_ind+1]; + float dst_nz = normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - cx) * dst_z * ifx; + float dst_y = (float(dst_h) - cy) * dst_z * ify; + + double dst_noise = dst_z * dst_z; + + double point_noise = 1.0/sqrt(point_information); + + float tnx = im00*src_nx + im01*src_ny + im02*src_nz; + float tny = im10*src_nx + im11*src_ny + im12*src_nz; + float tnz = im20*src_nx + im21*src_ny + im22*src_nz; + + double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; + + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE + float px = m00*dst_x + m01*dst_y + m02*dst_z + m03; + float py = m10*dst_x + m11*dst_y + m12*dst_z + m13; + float pz = m20*dst_x + m21*dst_y + m22*dst_z + m23; + + float pnx = m00*dst_nx + m01*dst_ny + m02*dst_nz; + float pny = m10*dst_nx + m11*dst_ny + m12*dst_nz; + float pnz = m20*dst_nx + m21*dst_ny + m22*dst_nz; + + float pb = rgbdata[3*ind+0]; + float pg = rgbdata[3*ind+1]; + float pr = rgbdata[3*ind+2]; + + Vector3f pxyz (px ,py ,pz ); + Vector3f pnxyz (pnx,pny,pnz); + Vector3f prgb (pr ,pg ,pb ); + float weight = 1.0/(dst_noise*dst_noise); + superpoint sp2 = superpoint(pxyz,pnxyz,prgb, weight, weight, frameid); + sp.merge(sp2); + isfused[dst_ind] = true; + } + } + } + } + + int nr_fused = 0; + int nr_mask = 0; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + nr_fused += isfused[ind]; + nr_mask += maskvec[ind] > 0; + } + } + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(!isfused[ind] && maskvec[ind]){ + float z = idepth*float(depthdata[ind]); + float nx = normalsdata[3*ind+0]; + + if(z > 0 && nx != 2){ + float ny = normalsdata[3*ind+1]; + float nz = normalsdata[3*ind+2]; + + float x = (w - cx) * z * ifx; + float y = (h - cy) * z * ify; + + double noise = z * z; + + float px = m00*x + m01*y + m02*z + m03; + float py = m10*x + m11*y + m12*z + m13; + float pz = m20*x + m21*y + m22*z + m23; + float pnx = m00*nx + m01*ny + m02*nz; + float pny = m10*nx + m11*ny + m12*nz; + float pnz = m20*nx + m21*ny + m22*nz; + + float pb = rgbdata[3*ind+0]; + float pg = rgbdata[3*ind+1]; + float pr = rgbdata[3*ind+2]; + + Vector3f pxyz (px ,py ,pz ); + Vector3f pnxyz (pnx,pny,pnz); + Vector3f prgb (pr ,pg ,pb ); + float weight = 1.0/(noise*noise); + spvec.push_back(superpoint(pxyz,pnxyz,prgb, weight, weight, frameid)); + } + } + } + } + + delete[] isfused; +} + +void Model::showHistory(boost::shared_ptr viewer){ + //viewer->setBackgroundColor(0.01*(rand()%100),0.01*(rand()%100),0.01*(rand()%100)); + for(unsigned int i = 0; i < submodels.size(); i++){ + + + pcl::PointCloud::Ptr room_cloud (new pcl::PointCloud); + + + for(unsigned int j = 0; j < submodels[i]->frames.size(); j++){ + bool * maskvec = submodels[i]->modelmasks[j]->maskvec; + unsigned char * rgbdata = (unsigned char *)(submodels[i]->frames[j]->rgb.data); + unsigned short * depthdata = (unsigned short *)(submodels[i]->frames[j]->depth.data); + float * normalsdata = (float *)(submodels[i]->frames[j]->normals.data); + + Camera * camera = submodels[i]->frames[j]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + Eigen::Matrix4d p = submodels[i]->relativeposes[j]; + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(!maskvec[ind]){ + float z = idepth*float(depthdata[ind]); + float nx = normalsdata[3*ind+0]; + + if(z > 0 && nx != 2){ + float ny = normalsdata[3*ind+1]; + float nz = normalsdata[3*ind+2]; + + float x = (w - cx) * z * ifx; + float y = (h - cy) * z * ify; + + pcl::PointXYZRGB po; + po.x = m00*x + m01*y + m02*z + m03; + po.y = m10*x + m11*y + m12*z + m13; + po.z = m20*x + m21*y + m22*z + m23; +// float pnx = m00*nx + m01*ny + m02*nz; +// float pny = m10*nx + m11*ny + m12*nz; +// float pnz = m20*nx + m21*ny + m22*nz; + + po.b = rgbdata[3*ind+0]; + po.g = rgbdata[3*ind+1]; + po.r = rgbdata[3*ind+2]; + + + room_cloud->points.push_back(po); + } + } + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (room_cloud, pcl::visualization::PointCloudColorHandlerRGBField(room_cloud), "room_cloud"); + + pcl::PointCloud::Ptr cloud = submodels[i]->getPCLcloud(1, true); + for(unsigned int j = 0; j < cloud->points.size(); j++){ + cloud->points[j].r = 255; + cloud->points[j].g = 0; + cloud->points[j].b = 0; + } + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "submodel"); + + pcl::PointCloud::Ptr cloud2 = getPCLcloud(1, true); + + pcl::PointCloud::Ptr tcloud (new pcl::PointCloud ()); + pcl::transformPointCloud (*cloud2, *tcloud, Eigen::Affine3d(submodels_relativeposes[i]).inverse()); + viewer->addPointCloud (tcloud, pcl::visualization::PointCloudColorHandlerRGBField(tcloud), "model"); + viewer->spin(); + + } +} + +void Model::addAllSuperPoints(std::vector & spvec, Eigen::Matrix4d pose){ + for(unsigned int i = 0; i < frames.size(); i++){ + addSuperPoints(spvec, pose*relativeposes[i], frames[i], modelmasks[i]); + } + + for(unsigned int i = 0; i < submodels.size(); i++){ + submodels[i]->addAllSuperPoints(spvec, pose*submodels_relativeposes[i]); + } +} + +void Model::recomputeModelPoints(Eigen::Matrix4d pose){ // for(unsigned int i = 0; i < frames.size(); i++){ // bool res = testFrame(i); // } points.clear(); - for(unsigned int i = 0; i < frames.size(); i++){ - addPointsToModel(frames[i],modelmasks[i],relativeposes[i]); - } + addAllSuperPoints(points,pose); + +// vector spvec; +// addAllSuperPoints(spvec,pose); +// points = spvec; } void Model::addPointsToModel(RGBDFrame * frame, ModelMask * modelmask, Eigen::Matrix4d p){ @@ -193,6 +454,12 @@ void Model::merge(Model * model, Eigen::Matrix4d p){ frames.push_back(model->frames[i]); modelmasks.push_back(model->modelmasks[i]); } + + for(unsigned int i = 0; i < model->submodels.size(); i++){ + submodels_relativeposes.push_back(p * model->submodels_relativeposes[i]); + submodels.push_back(model->submodels[i]); + } + recomputeModelPoints(); } @@ -270,6 +537,25 @@ pcl::PointCloud::Ptr Model::getPCLnormalcloud(int step, pcl::PointCloud::Ptr Model::getPCLcloud(int step, bool color){ pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); + for(unsigned int i = 0; i < points.size(); i+=step){ + superpoint & sp = points[i]; + pcl::PointXYZRGB p; + p.x = sp.point(0); + p.y = sp.point(1); + p.z = sp.point(2); + + if(color){ + p.b = 0; + p.g = 255; + p.r = 0; + }else{ + p.r = sp.feature(0); + p.g = sp.feature(1); + p.b = sp.feature(2); + } + cloud_ptr->points.push_back(p); + } + return cloud_ptr; // if(color){ // for(unsigned int i = 0; i < points.size(); i+=step){ // superpoint & sp = points[i]; @@ -289,6 +575,7 @@ pcl::PointCloud::Ptr Model::getPCLcloud(int step, bool color){ // cloud_ptr->points.push_back(p); // } // }else{ +/* std::map mymapR; std::map mymapG; std::map mymapB; @@ -368,6 +655,7 @@ pcl::PointCloud::Ptr Model::getPCLcloud(int step, bool color){ } //} return cloud_ptr; +*/ } void Model::save(std::string path){ diff --git a/quasimodo/quasimodo_models/src/model/ModelMask.cpp b/quasimodo/quasimodo_models/src/model/ModelMask.cpp index c3294bce..1c79a0d1 100644 --- a/quasimodo/quasimodo_models/src/model/ModelMask.cpp +++ b/quasimodo/quasimodo_models/src/model/ModelMask.cpp @@ -25,24 +25,57 @@ ModelMask::ModelMask(cv::Mat mask_){ - for(unsigned int w = 1; w < 639; w++){ - for(unsigned int h = 1; h < 479; h++){ +// for(unsigned int w = 1; w < 639; w++){ +// for(unsigned int h = 1; h < 479; h++){ +// if(maskdata[h*640+w] != 0){ +// testw.push_back(w); +// testh.push_back(h); +// } +// } +// } + +// cv::Mat combined; +// combined.create(height,width,CV_8UC3); +// unsigned char * combineddata = (unsigned char *)combined.data; +// for(int j = 0; j < 3*width*height; j++){combineddata[j] = 0;} + //cv::Mat combined = mask.clone(); + + + int step = 0; + for(int w = step; w < 639-step; w++){ + for(int h = step; h < 479-step; h++){ if(maskdata[h*640+w] != 0){ - testw.push_back(w); - testh.push_back(h); +// combineddata[3*(h*640+w) + 0] = 0; +// combineddata[3*(h*640+w) + 1] = 0; +// combineddata[3*(h*640+w) + 2] = 255; +// printf("%i %i-> ",w,h); + bool isok = true; + for(int w2 = w-step; w2 <= w+step; w2++){ + for(int h2 = h-step; h2 <= h+step; h2++){ +// printf("(%i %i , %i -> %i) ",w2,h2,isok,maskdata[h2*640+w2] != 0); + isok = isok && (maskdata[h2*640+w2] != 0); + } + } +// printf("\n"); +// printf("\n"); +// printf("\n"); +// printf("\n"); + if(isok){ +// combineddata[3*(h*640+w) + 0] = 0; +// combineddata[3*(h*640+w) + 1] = 255; +// combineddata[3*(h*640+w) + 2] = 0; + testw.push_back(w); + testh.push_back(h); + } } } } - for(unsigned int w = 1; w < 639; w++){ - for(unsigned int h = 1; h < 479; h++){ - if(maskdata[h*640+w] != 0){ - testw.push_back(w); - testh.push_back(h); - } - } - } +// cv::imshow( "combined", combined ); +// cv::waitKey(0); + +//exit(0); for(unsigned int i = 0; i < testw.size(); i++){ int ind = rand() % testw.size(); int tw = testw[i]; diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 4b6fe1b9..3d3b3fcf 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -12,6 +12,12 @@ #include #include +#include "opencv2/core/core.hpp" +#include "opencv2/features2d/features2d.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/calib3d/calib3d.hpp" +#include "opencv2/nonfree/nonfree.hpp" + typedef boost::property edge_weight_property; typedef boost::property vertex_name_property; using Graph = boost::adjacency_list; @@ -25,6 +31,11 @@ using namespace std; namespace reglib { +double mysign(double v){ + if(v < 0){return -1;} + return 1; +} + double getTime(){ struct timeval start1; gettimeofday(&start1, NULL); @@ -163,12 +174,896 @@ void ModelUpdater::fuse(Model * model2, Eigen::Matrix4d guess, double uncertanit UpdatedModels ModelUpdater::fuseData(FusionResults * f, Model * model1,Model * model2){return UpdatedModels();} -void ModelUpdater::refine(double reg,bool useFullMask){ +bool ModelUpdater::isRefinementNeeded(){ + vector > occlusionScores; + occlusionScores.resize(model->frames.size()); + for(unsigned int i = 0; i < model->frames.size(); i++){occlusionScores[i].resize(model->frames.size());} + + + for(unsigned int i = 0; i < model->frames.size(); i++){ + for(unsigned int j = i+1; j < model->frames.size(); j++){ + Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; + occlusionScores[j][i] = computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,false); + occlusionScores[i][j] = computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,false); + } + } + + + std::vector > scores = getScores(occlusionScores); + std::vector partition = getPartition(scores,2,5,2); + + bool failed = false; + for(unsigned int i = 0; i < partition.size(); i++){failed = failed | (partition[i]!=0);} + + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + if(scores[i][j] > 0){printf(" ");} + printf("%5.5f ",0.0001*scores[i][j]); +// if(scores[i][j] < 0){ +// Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; +// computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,true); +// computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,true); +// } + } + printf("\n"); + } + + printf("partition"); + for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} + printf("\n"); + return failed; +} + +OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step, bool debugg){ +// printf("start:: %s::%i\n",__FILE__,__LINE__); + OcclusionScore oc; + + unsigned char * dst_maskdata = (unsigned char *)(cm->mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); + float * dst_normalsdata = (float *)(cf->normals.data); + + float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); + float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); + float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); + + Camera * dst_camera = cf->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + int nr_data = spvec.size(); + + std::vector residuals; + std::vector debugg_src_inds; + std::vector debugg_dst_inds; + std::vector weights; + residuals.reserve(nr_data); + if(debugg){ + debugg_src_inds.reserve(nr_data); + debugg_dst_inds.reserve(nr_data); + } + weights.reserve(nr_data); + + for(unsigned int ind = 0; ind < spvec.size();ind+=step){ + superpoint & sp = spvec[ind]; + + float src_x = sp.point(0); + float src_y = sp.point(1); + float src_z = sp.point(2); + + float src_nx = sp.normal(0); + float src_ny = sp.normal(1); + float src_nz = sp.normal(2); + + float point_information = sp.point_information; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + double dst_noise = dst_z * dst_z; + double point_noise = 1.0/sqrt(point_information); + + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; + + double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); + double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); + + residuals.push_back(d); + weights.push_back(angle_dst*angle_dst*angle_dst); + if(debugg){ + debugg_src_inds.push_back(ind); + debugg_dst_inds.push_back(dst_ind); + } + } + } + } + +// DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); +// func->maxp = 1.0; +// func->update_size = true; +// func->zeromean = true; +// func->startreg = 0.0001; +// func->debugg_print = debugg; +// func->bidir = true; +// func->maxnoise = pred; +// func->reset(); + + DistanceWeightFunction2 * func = new DistanceWeightFunction2(); + func->f = THRESHOLD; + func->p = 0.01; + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); + for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} + func->computeModel(X); + + Eigen::VectorXd W = func->getProbs(X); + + delete func; + + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + oc.score += weight*weights.at(i); + oc.occlusions += ocl*weights.at(i); + } + + + if(debugg){ + //cf->show(true); + + pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + dcloud->points.resize(dst_width*dst_height); + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + unsigned int dst_ind = dst_h*dst_width+dst_w; + float z = dst_idepth*float(dst_depthdata[dst_ind]); + if(z > 0){ + float x = (float(dst_w) - dst_cx) * z * dst_ifx; + float y = (float(dst_h) - dst_cy) * z * dst_ify; + dcloud->points[dst_ind].x = x; + dcloud->points[dst_ind].y = y; + dcloud->points[dst_ind].z = z; + dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; + dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; + dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; + if(dst_w % 10 == 0 && dst_h % 10 == 0){ + //printf("%i %i -> %4.4f %4.4f %4.4f\n",dst_w,dst_h,dst_normalsdata[3*dst_ind+0],dst_h,dst_normalsdata[3*dst_ind+1],dst_h,dst_normalsdata[3*dst_ind+2]); + } + //if(false && dst_maskdata[dst_ind] == 255){ + if(dst_maskdata[dst_ind] == 255){ + dcloud->points[dst_ind].r = 255; + dcloud->points[dst_ind].g = 000; + dcloud->points[dst_ind].b = 255; + } + } + } + } + + oc.print(); + pcl::PointCloud::Ptr scloud (new pcl::PointCloud); + for(unsigned int i = 0; i < spvec.size(); i++){ + superpoint & sp = spvec[i]; + pcl::PointXYZRGBNormal p; + p.x = sp.point(0); + p.y = sp.point(1); + p.z = sp.point(2); + + float tx = m00*p.x + m01*p.y + m02*p.z + m03; + float ty = m10*p.x + m11*p.y + m12*p.z + m13; + float tz = m20*p.x + m21*p.y + m22*p.z + m23; + + p.x = tx; + p.y = ty; + p.z = tz; + + p.normal_x = sp.normal(0); + p.normal_y = sp.normal(1); + p.normal_z = sp.normal(2); + p.r = sp.feature(0); + p.g = sp.feature(1); + p.b = sp.feature(2); + + scloud->points.push_back(p); + } + +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); +// viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); +// viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->removeAllShapes(); + + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + unsigned int src_ind = debugg_src_inds[i]; + unsigned int dst_ind = debugg_dst_inds[i]; + if(ocl > 0.01 || weight > 0.01){ + scloud->points[src_ind].r = 255.0*ocl*weights.at(i); + scloud->points[src_ind].g = 255.0*weight*weights.at(i); + scloud->points[src_ind].b = 0; + + if(i % 100 == 0){ + char buf [1024]; + sprintf(buf,"line%i",i); + viewer->addLine (scloud->points[src_ind], dcloud->points[dst_ind],buf); + } + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud"); + + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + viewer->removeAllShapes(); + + } +//printf("stop :: %s::%i\n",__FILE__,__LINE__); + return oc; +} + +OcclusionScore ModelUpdater::computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp, int step, bool debugg){ + OcclusionScore ocs; + for(unsigned int i = 0; i < cp.size(); i++){ + ocs.add(computeOcclusionScore(mod->points,cp[i].inverse()*rp,cf[i],cm[i],step,debugg)); + } + return ocs; +} + + +OcclusionScore ModelUpdater::computeOcclusionScore(Model * model1, Model * model2, Matrix4d rp, int step, bool debugg){ + OcclusionScore ocs; +// ocs.add(computeOcclusionScore(model1, model2->rep_relativeposes,model2->rep_frames,model2->rep_modelmasks,rp.inverse(),step,debugg)); +// ocs.add(computeOcclusionScore(model2, model1->rep_relativeposes,model1->rep_frames,model1->rep_modelmasks,rp,step,debugg)); + ocs.add(computeOcclusionScore(model1, model2->relativeposes,model2->frames,model2->modelmasks,rp.inverse(),step,debugg)); + ocs.add(computeOcclusionScore(model2, model1->relativeposes,model1->frames,model1->modelmasks,rp,step,debugg)); + return ocs; +} + +vector > ModelUpdater::computeOcclusionScore(vector models, vector rps, int step, bool debugg){ + unsigned int nr_models = models.size(); + vector > occlusionScores; + occlusionScores.resize(nr_models); + for(unsigned int i = 0; i < nr_models; i++){occlusionScores[i].resize(nr_models);} + + for(unsigned int i = 0; i < nr_models; i++){ + for(unsigned int j = i+1; j < nr_models; j++){ + //printf("%5.5f %5.5f\n",double(i+1)/double(nr_models),double(j+1)/double(nr_models)); + Eigen::Matrix4d relative_pose = rps[i].inverse() * rps[j]; + occlusionScores[i][j] = computeOcclusionScore(models[i],models[j], relative_pose, step, debugg); + occlusionScores[j][i] = occlusionScores[i][j]; + } + } + + return occlusionScores; +} + +double ModelUpdater::computeOcclusionScoreCosts(vector models){ + double total = 0; + + unsigned int nr_models = models.size(); + for(unsigned int i = 0; i < nr_models; i++){ + for(unsigned int j = 0; j < nr_models; j++){ + if(i == j){continue;} + total += models[j]->rep_frames.size() * models[i]->points.size(); + } + } + + return total; +} + +void ModelUpdater::addModelsToVector(vector & models, vector & rps, Model * model, Matrix4d rp){ + if(model->frames.size() > 0){ + models.push_back(model); + rps.push_back(rp); + } + + for(unsigned int i = 0; i < model->submodels.size(); i++){ + addModelsToVector(models,rps,model->submodels[i], rp*model->submodels_relativeposes[i]); + } +} + +pcl::PointCloud::Ptr ModelUpdater::getPCLnormalcloud(vector & points){ + pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); + for(unsigned int i = 0; i < points.size(); i++){ + superpoint & sp = points[i]; + pcl::PointXYZRGBNormal p; + p.x = sp.point(0); + p.y = sp.point(1); + p.z = sp.point(2); + + p.normal_x = sp.normal(0); + p.normal_y = sp.normal(1); + p.normal_z = sp.normal(2); + p.r = sp.feature(0); + p.g = sp.feature(1); + p.b = sp.feature(2); + + cloud_ptr->points.push_back(p); + } + return cloud_ptr; +} + +bool ModelUpdater::refineIfNeeded(){ + + if(isRefinementNeeded()){ + MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1); + massreg->timeout = massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = 1; + massreg->maskstep = 4; + massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); + + massreg->nomask = true; + massreg->stopval = 0.001; + + massreg->setData(model->frames,model->modelmasks); + MassFusionResults mfr = massreg->getTransforms(model->relativeposes); + model->relativeposes = mfr.poses; + + isRefinementNeeded(); + } + + + MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1); + massreg->timeout = massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = 1; + massreg->maskstep = 4; + massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); + + massreg->nomask = true; + massreg->stopval = 0.001; + + massreg->setData(model->frames,model->modelmasks); + MassFusionResults mfr = massreg->getTransforms(model->relativeposes); + model->relativeposes = mfr.poses; + + vector spvec = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); + vector cp; + vector cf; + vector cm; + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + cp.push_back(model->relativeposes[i]); + cf.push_back(model->frames[i]); + cm.push_back(model->modelmasks[i]); + } + getGoodCompareFrames(cp,cf,cm); + + vector spvec2 = getSuperPoints(cp,cf,cm,1,true); + return true; +} + +void ModelUpdater::makeInitialSetup(){ + + if(model->relativeposes.size() <= 1){ + model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); + + vector cp; + vector cf; + vector cm; + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + cp.push_back(model->relativeposes[i]); + cf.push_back(model->frames[i]); + cm.push_back(model->modelmasks[i]); + } + getGoodCompareFrames(cp,cf,cm); + + model->rep_relativeposes = cp; + model->rep_frames = cf; + model->rep_modelmasks = cm; + return ; + } + + int minHessian = 400; + + cv::SurfFeatureDetector detector( minHessian ); + cv::SurfDescriptorExtractor extractor; + + + //cv::ORB orb = cv::ORB(250);//,1.2f, 1, 3, 0,2, cv::ORB::HARRIS_SCORE, 31); + cv::ORB orb = cv::ORB(10);//,1.2f, 1, 3, 0,2, cv::ORB::HARRIS_SCORE, 31); +// for(unsigned int i = 0; i < model->frames.size(); i++){ +// std::vector keypoints; +// cv::Mat descriptors; +// //orb(model->frames[i]->rgb, model->modelmasks[i]->getMask(), keypoints, descriptors); +// orb(model->frames[i]->rgb, cv::Mat(), keypoints, descriptors); +// model->all_keypoints.push_back(keypoints); +// model->all_descriptors.push_back(descriptors); + +// printf("keypoints: %i\n",keypoints.size()); + + +// cv::Mat descriptors_surf; +// std::vector keypoints_surf; + +// detector.detect(model->frames[i]->rgb, keypoints_surf, model->modelmasks[i]->getMask() ); +// extractor.compute( model->frames[i]->rgb, keypoints_surf, descriptors_surf ); +// model->all_keypoints.push_back(keypoints_surf); +// model->all_descriptors.push_back(descriptors_surf); + +//// printf("keypoints: %i\n",keypoints.size()); +//// cv::Mat out; +//// cv::drawKeypoints(model->frames[i]->rgb, keypoints_surf, out, cv::Scalar::all(255)); +//// cv::imshow("Kpts", out); +//// cv::waitKey(0); +// } + + MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); + massreg->timeout = 4*massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = 1;//1; + massreg->maskstep = std::max(1,int(0.5+0.2*double(model->frames.size()))); + massreg->nomaskstep = std::max(5,int(0.5+1.0*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg->nomask = true; + massreg->stopval = 0.0001; + + //massreg->setData(model->frames,model->modelmasks); + massreg->addModelData(model, false); + std::vector p = model->submodels_relativeposes; + p.insert(p.end(), model->relativeposes.begin(), model->relativeposes.end()); + + + //MassFusionResults mfr = massreg->getTransforms(model->relativeposes); + MassFusionResults mfr = massreg->getTransforms(p); + + + model->relativeposes.clear();// = mfr.poses; + model->relativeposes.insert( model->relativeposes.end(), mfr.poses.begin()+model->submodels.size(), mfr.poses.end()); + + model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); + + vector cp; + vector cf; + vector cm; + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + cp.push_back(model->relativeposes[i]); + cf.push_back(model->frames[i]); + cm.push_back(model->modelmasks[i]); + } + getGoodCompareFrames(cp,cf,cm); + + model->rep_relativeposes = cp; + model->rep_frames = cf; + model->rep_modelmasks = cm; +} + +void ModelUpdater::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, int type, bool debugg){ + bool * maskvec = modelmask->maskvec; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + unsigned int frameid = frame->id; + + Matrix4d ip = p.inverse(); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + float im00 = ip(0,0); float im01 = ip(0,1); float im02 = ip(0,2); float im03 = ip(0,3); + float im10 = ip(1,0); float im11 = ip(1,1); float im12 = ip(1,2); float im13 = ip(1,3); + float im20 = ip(2,0); float im21 = ip(2,1); float im22 = ip(2,2); float im23 = ip(2,3); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + const unsigned int dst_width2 = camera->width - 2; + const unsigned int dst_height2 = camera->height - 2; + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float fx = camera->fx; + const float fy = camera->fy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + bool * isfused = new bool[width*height]; + for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} + + + for(unsigned int ind = 0; ind < spvec.size();ind++){ + superpoint & sp = spvec[ind]; + + float src_x = sp.point(0); + float src_y = sp.point(1); + float src_z = sp.point(2); + + float src_nx = sp.normal(0); + float src_ny = sp.normal(1); + float src_nz = sp.normal(2); + + float src_r = sp.feature(0); + float src_g = sp.feature(1); + float src_b = sp.feature(2); + + double point_information = sp.point_information; + double feature_information = sp.feature_information; + + float tx = im00*src_x + im01*src_y + im02*src_z + im03; + float ty = im10*src_x + im11*src_y + im12*src_z + im13; + float tz = im20*src_x + im21*src_y + im22*src_z + im23; + + float itz = 1.0/tz; + float dst_w = fx*tx*itz + cx; + float dst_h = fy*ty*itz + cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * width + unsigned(dst_w+0.5); + + float dst_z = idepth*float(depthdata[dst_ind]); + float dst_nx = normalsdata[3*dst_ind+0]; + if(!isfused[dst_ind] && dst_z > 0 && dst_nx != 2){ + float dst_ny = normalsdata[3*dst_ind+1]; + float dst_nz = normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - cx) * dst_z * ifx; + float dst_y = (float(dst_h) - cy) * dst_z * ify; + + double dst_noise = dst_z * dst_z; + + double point_noise = 1.0/sqrt(point_information); + + float tnx = im00*src_nx + im01*src_ny + im02*src_nz; + float tny = im10*src_nx + im11*src_ny + im12*src_nz; + float tnz = im20*src_nx + im21*src_ny + im22*src_nz; + + double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; + + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE + float px = m00*dst_x + m01*dst_y + m02*dst_z + m03; + float py = m10*dst_x + m11*dst_y + m12*dst_z + m13; + float pz = m20*dst_x + m21*dst_y + m22*dst_z + m23; + + float pnx = m00*dst_nx + m01*dst_ny + m02*dst_nz; + float pny = m10*dst_nx + m11*dst_ny + m12*dst_nz; + float pnz = m20*dst_nx + m21*dst_ny + m22*dst_nz; + + float pb = rgbdata[3*ind+0]; + float pg = rgbdata[3*ind+1]; + float pr = rgbdata[3*ind+2]; + + Vector3f pxyz (px ,py ,pz ); + Vector3f pnxyz (pnx,pny,pnz); + Vector3f prgb (pr ,pg ,pb ); + float weight = 1.0/(dst_noise*dst_noise); + superpoint sp2 = superpoint(pxyz,pnxyz,prgb, weight, weight, frameid); + sp.merge(sp2); + isfused[dst_ind] = true; + } + } + } + } + + int nr_fused = 0; + int nr_mask = 0; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + nr_fused += isfused[ind]; + nr_mask += maskvec[ind] > 0; + } + } + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(!isfused[ind] && maskvec[ind]){ + float z = idepth*float(depthdata[ind]); + float nx = normalsdata[3*ind+0]; + + if(z > 0 && nx != 2){ + float ny = normalsdata[3*ind+1]; + float nz = normalsdata[3*ind+2]; + + float x = (w - cx) * z * ifx; + float y = (h - cy) * z * ify; + + double noise = z * z; + + float px = m00*x + m01*y + m02*z + m03; + float py = m10*x + m11*y + m12*z + m13; + float pz = m20*x + m21*y + m22*z + m23; + float pnx = m00*nx + m01*ny + m02*nz; + float pny = m10*nx + m11*ny + m12*nz; + float pnz = m20*nx + m21*ny + m22*nz; + + float pb = rgbdata[3*ind+0]; + float pg = rgbdata[3*ind+1]; + float pr = rgbdata[3*ind+2]; + + Vector3f pxyz (px ,py ,pz ); + Vector3f pnxyz (pnx,pny,pnz); + Vector3f prgb (pr ,pg ,pb ); + float weight = 1.0/(noise*noise); + spvec.push_back(superpoint(pxyz,pnxyz,prgb, weight, weight, frameid)); + } + } + } + } + + delete[] isfused; + + if(debugg){ + pcl::PointCloud::Ptr cloud = getPCLnormalcloud(spvec); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } +} + +vector ModelUpdater::getSuperPoints(vector cp, vector cf, vector cm, int type, bool debugg){ + vector spvec; + for(unsigned int i = 0; i < cp.size(); i++){addSuperPoints(spvec, cp[i], cf[i], cm[i], type,debugg);} + return spvec; +} + +double ModelUpdater::getCompareUtility(Matrix4d p, RGBDFrame* frame, ModelMask* mask, vector & cp, vector & cf, vector & cm){ + unsigned char * src_maskdata = (unsigned char *)(mask->mask.data); + unsigned char * src_rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame->depth.data); + float * src_normalsdata = (float *)(frame->normals.data); + + Camera * src_camera = frame->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + std::vector test_x; + std::vector test_y; + std::vector test_z; + std::vector test_nx; + std::vector test_ny; + std::vector test_nz; + + std::vector & testw = mask->testw; + std::vector & testh = mask->testh; + + for(unsigned int ind = 0; ind < testw.size();ind++){ + unsigned int src_w = testw[ind]; + unsigned int src_h = testh[ind]; + + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] == 255){ + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + test_x.push_back(src_x); + test_y.push_back(src_y); + test_z.push_back(src_z); + test_nx.push_back(src_nx); + test_ny.push_back(src_ny); + test_nz.push_back(src_nz); + } + } + } + + bool debugg = false; + unsigned int nrdata_start = test_x.size(); + + for(unsigned int c = 0; c < cp.size();c++){ + RGBDFrame* dst = cf[c]; + if(dst == frame){continue;} + ModelMask* dst_modelmask = cm[c]; + unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); + float * dst_normalsdata = (float *)(dst->normals.data); + + Matrix4d rp = cp[c].inverse()*p; + + float m00 = rp(0,0); float m01 = rp(0,1); float m02 = rp(0,2); float m03 = rp(0,3); + float m10 = rp(1,0); float m11 = rp(1,1); float m12 = rp(1,2); float m13 = rp(1,3); + float m20 = rp(2,0); float m21 = rp(2,1); float m22 = rp(2,2); float m23 = rp(2,3); + + Camera * dst_camera = dst->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; +pcl::PointCloud::Ptr cloud (new pcl::PointCloud); +if(debugg){ + viewer->removeAllPointClouds(); + + pcl::PointCloud::Ptr cloud2 = dst->getPCLcloud(); + viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud2"); +} + + for(unsigned int ind = 0; ind < test_x.size();ind++){ + float src_nx = test_nx[ind]; + float src_ny = test_ny[ind]; + float src_nz = test_nz[ind]; + + float src_x = test_x[ind]; + float src_y = test_y[ind]; + float src_z = test_z[ind]; + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + pcl::PointXYZRGBNormal point; + if(debugg){ + point.x = tx; + point.y = ty; + point.z = tz; + point.r = 255; + point.g = 0; + point.b = 0; + } + + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + double dst_noise = dst_z * dst_z; + double point_noise = src_z * src_z; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; + + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE + //if(d < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + test_nx[ind] = test_nx.back(); test_nx.pop_back(); + test_ny[ind] = test_ny.back(); test_ny.pop_back(); + test_nz[ind] = test_nz.back(); test_nz.pop_back(); + test_x[ind] = test_x.back(); test_x.pop_back(); + test_y[ind] = test_y.back(); test_y.pop_back(); + test_z[ind] = test_z.back(); test_z.pop_back(); + ind--; +if(debugg){ + point.r = 0; + point.g = 255; + point.b = 0; +} + } + } + } +if(debugg){ + cloud->points.push_back(point); +} + } + +if(debugg){ + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); +} + } + return double(test_x.size())/double(nrdata_start); +} + +void ModelUpdater::getGoodCompareFrames(vector & cp, vector & cf, vector & cm){ + while(true){ + double worst = 1; + int worst_id = -1; + for(unsigned int i = 0; i < cp.size(); i++){ + double score = getCompareUtility(cp[i], cf[i], cm[i], cp, cf, cm); + //printf("%3.3i -> %f\n",i,score); + if(score < worst){ + worst = score; + worst_id = i; + } + } + + if(worst_id < 0){break;} + if(worst < 0.1){//worst frame can to 90% be represented by other frames + cp[worst_id] = cp.back(); + cp.pop_back(); + + cf[worst_id] = cf.back(); + cf.pop_back(); + + cm[worst_id] = cm.back(); + cm.pop_back(); + //printf("removing %i\n",worst_id); + }else{break;} + } +} + +void ModelUpdater::refine(double reg,bool useFullMask, int visualization){ //return ; printf("void ModelUpdater::refine()\n"); - std::vector > ocs = getOcclusionScores(model->relativeposes, model->frames,model->modelmasks,false); +printf("%s::%i\n",__FILE__,__LINE__); + vector > ocs = computeOcclusionScore(model->submodels,model->submodels_relativeposes,1,false); +printf("%s::%i\n",__FILE__,__LINE__); std::vector > scores = getScores(ocs); +printf("%s::%i\n",__FILE__,__LINE__); double sumscore_bef = 0; for(unsigned int i = 0; i < scores.size(); i++){ @@ -181,60 +1076,19 @@ void ModelUpdater::refine(double reg,bool useFullMask){ MassRegistrationPPR * massreg = new MassRegistrationPPR(reg); massreg->timeout = massreg_timeout; massreg->viewer = viewer; - massreg->visualizationLvl = 0; + massreg->visualizationLvl = 1; massreg->maskstep = 4;//std::max(1,int(0.5+0.02*double(model->frames.size()))); massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); - printf("maskstep: %i nomaskstep: %i\n",massreg->maskstep,massreg->nomaskstep); - - //massreg->type = PointToPoint; - if(useFullMask){ -// massreg->visualizationLvl = 0; -// MassRegistrationPPRColor * massregC = new MassRegistrationPPRColor(reg); -// massregC->viewer = viewer; -// massregC->visualizationLvl = 1; -// massregC->steps = 8; -// massregC->stopval = 0.001; -// massregC->setData(model->frames,model->modelmasks); -// mfr = massregC->getTransforms(model->relativeposes); -// model->relativeposes = mfr.poses; -// exit(0); - - massreg->setData(model->frames,model->modelmasks); - massreg->nomask = false; -// massreg->visualizationLvl = 0; - massreg->stopval = 0.001; - massreg->steps = 10; - - mfr = massreg->getTransforms(model->relativeposes); - //model->relativeposes = mfr.poses; - //massreg->steps = 8; - //mfr = massreg->getTransforms(model->relativeposes); - //model->relativeposes = mfr.poses; -// exit(0); - -// massreg->visualizationLvl = 2; -// massreg->stopval = 0.002; -// massreg->steps = 4; -// mfr = massreg->getTransforms(model->relativeposes); -// model->relativeposes = mfr.poses; - - }else{ -printf("%s::%i\n",__FILE__,__LINE__); - exit(0); - - massreg->nomask = true; - massreg->visualizationLvl = 3; - massreg->steps = 10; - massreg->stopval = 0.001; - //massreg->setData(model->frames,model->masks); - massreg->setData(model->frames,model->modelmasks); - mfr = massreg->getTransforms(model->relativeposes); - //model->relativeposes = mfr.poses; - - } - - std::vector > ocs2 = getOcclusionScores(mfr.poses, model->frames,model->modelmasks,false); + massreg->nomask = true; + massreg->stopval = 0.001; + double step = model->submodels_relativeposes.size()*model->submodels_relativeposes.size(); + step *= 0.25; + step += 0.5; + massreg->maskstep = std::max(1.5,step); + massreg->addModelData(model); + mfr = massreg->getTransforms(model->submodels_relativeposes); + vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); std::vector > scores2 = getScores(ocs2); double sumscore_aft = 0; @@ -243,10 +1097,10 @@ printf("%s::%i\n",__FILE__,__LINE__); sumscore_aft += scores2[i][j]; } } + + printf("bef %f after %f\n",sumscore_bef,sumscore_aft); if(sumscore_aft >= sumscore_bef){ - model->relativeposes = mfr.poses; - model->scores = scores2; - model->total_scores = sumscore_aft; + model->submodels_relativeposes = mfr.poses; } } @@ -410,14 +1264,22 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * } std::vector residuals; + std::vector angleweights; std::vector weights; std::vector ws; std::vector hs; + + std::vector dst_ws; + std::vector dst_hs; + residuals.reserve(src_width*src_height); + angleweights.reserve(src_width*src_height); weights.reserve(src_width*src_height); if(debugg){ ws.reserve(src_width*src_height); hs.reserve(src_width*src_height); + dst_ws.reserve(dst_width*dst_height); + dst_hs.reserve(dst_width*dst_height); } double sum = 0; @@ -511,19 +1373,18 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * int src_ind = src_h*src_width+src_w; if(src_maskdata[src_ind] == 255){ - float z = src_idepth*float(src_depthdata[src_ind]); - float nx = src_normalsdata[3*src_ind+0]; - - if(z > 0 && nx != 2){ - float ny = src_normalsdata[3*src_ind+1]; - float nz = src_normalsdata[3*src_ind+2]; - - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; - - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + //if(src_z > 0){ + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; float itz = 1.0/tz; float dst_w = dst_fx*tx*itz + dst_cx; float dst_h = dst_fy*ty*itz + dst_cy; @@ -532,21 +1393,51 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - if(dst_z > 0){ - float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 - residuals.push_back(diff_z); - - float tnx = m00*nx + m01*ny + m02*nz; - float tny = m10*nx + m11*ny + m12*nz; - float tnz = m20*nx + m21*ny + m22*nz; + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - float angle = (tnx*dst_x+tny*dst_y+tnz*dst_z)/sqrt(dst_x*dst_x + dst_y*dst_y + dst_z*dst_z); - weights.push_back(1-angle); + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double info = (src_z*src_z+dst_z*dst_z); + + //double d = mysign(dst_z-tz)*fabs(dst_nx*(dst_x-tx) + dst_ny*(dst_y-ty) + dst_nz*(dst_z-tz)); + double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + residuals.push_back(d/info); + + //float diff_z = (dst_z-tz)/(src_z*src_z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 + //residuals.push_back(diff_z); + + + + + +// float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; +// float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + //float angle = (tnx*dst_x+tny*dst_y+tnz*dst_z)/sqrt(dst_x*dst_x + dst_y*dst_y + dst_z*dst_z); + //weights.push_back(1-angle); + //weights.push_back(ny*dst_ny); + + + double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); + double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); + + double dist_src = sqrt(src_x*src_x+src_y*src_y+src_z*src_z); + double angle_src = fabs((src_x*src_nx+src_y*src_ny+src_z*src_nz)/dist_src); + weights.push_back(angle_src * angle_dst); + if(debugg){ ws.push_back(src_w); hs.push_back(src_h); + + dst_ws.push_back(dst_w); + dst_hs.push_back(dst_h); } } } @@ -594,6 +1485,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); scloud->points.resize(src_width*src_height); dcloud->points.resize(dst_width*dst_height); +/* for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ for(unsigned int src_h = 0; src_h < src_height;src_h++){ int src_ind = src_h*src_width+src_w; @@ -643,7 +1535,8 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); viewer->spin(); viewer->removeAllPointClouds(); - + */ +/* for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ int dst_ind = dst_h*dst_width+dst_w; @@ -665,8 +1558,8 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * unsigned int src_ind = h * src_width + w; if(ocl > 0.01 || weight > 0.01){ scloud->points[src_ind].r = 255.0*ocl; - scloud->points[src_ind].g = 255.0*weight; - scloud->points[src_ind].b = 255.0*0; + scloud->points[src_ind].g = 0; + scloud->points[src_ind].b = 0; }else{ scloud->points[src_ind].x = 0; scloud->points[src_ind].y = 0; @@ -753,6 +1646,128 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); viewer->spin(); viewer->removeAllPointClouds(); +*/ +/* + for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] != 255){continue;} + float z = src_idepth*float(src_depthdata[src_ind]); + if(z > 0){ + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + scloud->points[src_ind].x = x; + scloud->points[src_ind].y = y; + scloud->points[src_ind].z = z; + scloud->points[src_ind].r = 0; + scloud->points[src_ind].g = 0; + scloud->points[src_ind].b = 0; + } + } + } +*/ + + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + unsigned int dst_ind = dst_h*dst_width+dst_w; + float z = dst_idepth*float(dst_depthdata[dst_ind]); + if(z > 0){ + float x = (float(dst_w) - dst_cx) * z * dst_ifx; + float y = (float(dst_h) - dst_cy) * z * dst_ify; + dcloud->points[dst_ind].x = x; + dcloud->points[dst_ind].y = y; + dcloud->points[dst_ind].z = z+2; + dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; + dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; + dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; + if(false && dst_maskdata[dst_ind] == 255){ + dcloud->points[dst_ind].r = 255; + dcloud->points[dst_ind].g = 000; + dcloud->points[dst_ind].b = 255; + } + } + } + } +// viewer->removeAllPointClouds(); +// //printf("%i showing results\n",__LINE__); +// viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); +// viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + + viewer->removeAllShapes(); + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + int w = ws[i]; + int h = hs[i]; + unsigned int src_ind = h * src_width + w; + + int dst_w = dst_ws[i]; + int dst_h = dst_hs[i]; + unsigned int dst_ind = dst_h * src_width + dst_w; + if(ocl > 0.01 || weight > 0.01){ + float z = src_idepth*float(src_depthdata[src_ind]); + float x = (float(w) - src_cx) * z * src_ifx; + float y = (float(h) - src_cy) * z * src_ify; + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + scloud->points[src_ind].x = tx; + scloud->points[src_ind].y = ty; + scloud->points[src_ind].z = tz+2; + + scloud->points[src_ind].r = 255.0*ocl*weights.at(i); + scloud->points[src_ind].g = 255.0*weight*weights.at(i); + scloud->points[src_ind].b = 0; + + char buf [1024]; + sprintf(buf,"line%i",i); + viewer->addLine (scloud->points[src_ind], dcloud->points[dst_ind],buf); + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud"); + + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + +/* + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + int w = ws[i]; + int h = hs[i]; + unsigned int src_ind = h * src_width + w; + if(ocl > 0.01 || weight > 0.01){ + scloud->points[src_ind].r = 255.0*(1-weights.at(i)); + scloud->points[src_ind].g = 255.0*weights.at(i); + scloud->points[src_ind].b = 0; + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->spin(); + viewer->removeAllPointClouds(); +*/ } /* if(debugg){ diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp index c543c30e..0997affb 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp @@ -24,88 +24,94 @@ ModelUpdaterBasicFuse::ModelUpdaterBasicFuse(Model * model_, Registration * regi ModelUpdaterBasicFuse::~ModelUpdaterBasicFuse(){} FusionResults ModelUpdaterBasicFuse::registerModel(Model * model2, Eigen::Matrix4d guess, double uncertanity){ - if(model->frames.size() > 0){ - printf("registerModel(%i %i)\n",int(model->id),int(model2->id)); + if(model->points.size() > 0 && model2->points.size() > 0){ registration->viewer = viewer; int step1 = std::max(int(model->frames.size())/1,1); int step2 = std::max(int(model2->frames.size())/1,1);//int step2 = 5;//std::min(int(model2->frames.size()),5); - +double register_setup_start = getTime(); CloudData * cd1 = model ->getCD(model->points.size()/step1); registration->setDst(cd1); CloudData * cd2 = model2->getCD(model2->points.size()/step2); registration->setSrc(cd2); +printf("register_setup_start: %5.5f\n",getTime()-register_setup_start); +double register_compute_start = getTime(); FusionResults fr = registration->getTransform(guess); - - printf("%i registerModel(%i %i)\n",__LINE__,int(model->id),int(model2->id)); +printf("register_compute_start: %5.5f\n",getTime()-register_compute_start); double best = -99999999999999; int best_id = -1; - for(unsigned int ca = 0; ca < fr.candidates.size() && ca < 150; ca++){ - Eigen::Matrix4d pose = fr.candidates[ca]; - std::vector current_poses; - std::vector current_frames; - //std::vector current_masks; - std::vector current_modelmasks; - - for(unsigned int i = 0; i < model->frames.size(); i++){ - current_poses.push_back( model->relativeposes[i]); - current_frames.push_back( model->frames[i]); - //current_masks.push_back( model->masks[i]); - current_modelmasks.push_back( model->modelmasks[i]); - } - - for(unsigned int i = 0; i < model2->frames.size(); i++){ - current_poses.push_back( pose * model2->relativeposes[i]); - current_frames.push_back( model2->frames[i]); - //current_masks.push_back( model2->masks[i]); - current_modelmasks.push_back( model2->modelmasks[i]); - } - - //std::vector > ocs = getOcclusionScores(current_poses, current_frames,current_masks,current_modelmasks); - std::vector > ocs = getOcclusionScores(current_poses, current_frames,current_modelmasks,false,10.0); - std::vector > scores = getScores(ocs); - std::vector partition = getPartition(scores,2,5,2); - - double sumscore1 = 0; - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - if(i < model->frames.size() && j < model->frames.size()){sumscore1 += scores[i][j];} - if(i >= model->frames.size() && j >= model->frames.size()){sumscore1 += scores[i][j];} - } - } - - double sumscore2 = 0; - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - if(partition[i] == partition[j]){sumscore2 += scores[i][j];} - } - } - - double improvement = sumscore2-sumscore1; - - if(improvement > best){ - best = improvement; - best_id = ca; - } - - printf("tested %i with score: %f\n",ca,sumscore2); - - //printf("sumscore before: %f\n",sumscore1); - //printf("sumscore after: %f\n",sumscore2); - //printf("%i improvement: %f\n",ca,improvement); - - if(improvement > 0){ - //printf("%i improvement: %f\n",ca,improvement); - //getOcclusionScores(current_poses, current_frames,current_masks,true); - } - } +double register_evaluate_start = getTime(); + +vector testmodels; +vector testrps; +addModelsToVector(testmodels,testrps,model,Eigen::Matrix4d::Identity()); +addModelsToVector(testmodels,testrps,model2,Eigen::Matrix4d::Identity()); + +int todo = fr.candidates.size(); +double expectedCost = double(todo)*computeOcclusionScoreCosts(testmodels); +printf("expectedCost: %f\n",expectedCost); + + + int step = 0.5 + expectedCost/11509168.5;// ~1 sec predicted + step = std::max(1,step); + + + +// printf("step: %i\n",step); + for(unsigned int ca = 0; ca < todo; ca++){ + Eigen::Matrix4d pose = fr.candidates[ca]; + + vector models; + vector rps; + + addModelsToVector(models,rps,model,Eigen::Matrix4d::Identity()); + unsigned int nr_models1 = models.size(); + addModelsToVector(models,rps,model2,pose); + + vector > ocs = computeOcclusionScore(models,rps,step,false); + std::vector > scores = getScores(ocs); + std::vector partition = getPartition(scores,2,5,2); + +// for(unsigned int i = 0; i < scores.size(); i++){ +// for(unsigned int j = 0; j < scores.size(); j++){ +// if(scores[i][j] >= 0){printf(" ");} +// printf("%5.5f ",0.00001*scores[i][j]); +// } +// printf("\n"); +// } +//printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); + + double sumscore1 = 0; + for(unsigned int i = 0; i < models.size(); i++){ + for(unsigned int j = 0; j < models.size(); j++){ + if(i < nr_models1 && j < nr_models1){sumscore1 += scores[i][j];} + if(i >= nr_models1 && j >= nr_models1){sumscore1 += scores[i][j];} + } + } + + double sumscore2 = 0; + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + if(partition[i] == partition[j]){sumscore2 += scores[i][j];} + } + } + + double improvement = sumscore2-sumscore1; + printf("improvement: %f\n",improvement); + + if(improvement > best){ + best = improvement; + best_id = ca; + } + } + +printf("register_evaluate_start: %5.5f\n",getTime()-register_evaluate_start); + if(best_id != -1){ fr.score = 9999999; fr.guess = fr.candidates[best_id]; } - - //printf("score: %6.6f\n",fr.score); delete cd1; delete cd2; @@ -116,250 +122,144 @@ FusionResults ModelUpdaterBasicFuse::registerModel(Model * model2, Eigen::Matrix void ModelUpdaterBasicFuse::fuse(Model * model2, Eigen::Matrix4d guess, double uncertanity){} + UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, Model * model2){ + UpdatedModels retval = UpdatedModels(); + Eigen::Matrix4d pose = f->guess; + printf("MODEL1 "); model1->print(); - printf("MODEL2 "); model2->print(); + printf("input pose\n"); + std::cout << pose << std::endl << std::endl; + +// std::vector current_poses; +// std::vector current_frames; +// std::vector current_modelmasks; + +// for(unsigned int i = 0; i < model1->frames.size(); i++){ +// current_poses.push_back( model1->relativeposes[i]); +// current_frames.push_back( model1->frames[i]); +// current_modelmasks.push_back( model1->modelmasks[i]); +// } + +// for(unsigned int i = 0; i < model2->frames.size(); i++){ +// current_poses.push_back( pose * model2->relativeposes[i]); +// current_frames.push_back( model2->frames[i]); +// current_modelmasks.push_back( model2->modelmasks[i]); +// } + + vector models; + vector rps; + addModelsToVector(models,rps,model,Eigen::Matrix4d::Identity()); + unsigned int nr_models1 = models.size(); + addModelsToVector(models,rps,model2,pose); + + double expectedCost = computeOcclusionScoreCosts(models); + printf("expectedCost: %f\n",expectedCost); + int step = 0.5 + expectedCost/(10.0*11509168.5);// ~10 sec predicted max time + step = std::max(1,step); + + vector > ocs = computeOcclusionScore(models,rps,step,false); + std::vector > scores = getScores(ocs); + std::vector partition = getPartition(scores,2,5,2); - UpdatedModels retval = UpdatedModels(); - Eigen::Matrix4d pose = f->guess; - - std::vector current_poses; - std::vector current_frames; - std::vector current_modelmasks; - - for(unsigned int i = 0; i < model1->frames.size(); i++){ - current_poses.push_back( model1->relativeposes[i]); - current_frames.push_back( model1->frames[i]); - current_modelmasks.push_back( model1->modelmasks[i]); - } - - for(unsigned int i = 0; i < model2->frames.size(); i++){ - current_poses.push_back( pose * model2->relativeposes[i]); - current_frames.push_back( model2->frames[i]); - current_modelmasks.push_back( model2->modelmasks[i]); + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + if(scores[i][j] >= 0){printf(" ");} + printf("%5.5f ",0.00001*scores[i][j]); + } + printf("\n"); } + printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); - double beforescore = model1->total_scores+model2->total_scores; -// printf("beforescore: %f\n",beforescore); - //std::vector > ocs = getOcclusionScores(current_poses, current_frames,current_masks,current_modelmasks,false); - std::vector > ocs = getOcclusionScores(current_poses, current_frames,current_modelmasks,false); - //std::vector > ocs = getOcclusionScores(current_poses, current_frames,current_modelmasks,true); - std::vector > scores = getScores(ocs); - std::vector partition = getPartition(scores,2,5,2); -// printf("model1\n"); - unsigned int frames1 = model1->scores.size(); - unsigned int frames2 = model2->scores.size(); double sumscore1 = 0; - for(unsigned int i = 0; i < frames1; i++){ - for(unsigned int j = 0; j < frames1; j++){ - sumscore1 += scores[i][j]; + for(unsigned int i = 0; i < models.size(); i++){ + for(unsigned int j = 0; j < models.size(); j++){ + if(i < nr_models1 && j < nr_models1){sumscore1 += scores[i][j];} + if(i >= nr_models1 && j >= nr_models1){sumscore1 += scores[i][j];} } } - -// printf("model2\n"); double sumscore2 = 0; - for(unsigned int i = 0; i < frames2; i++){ - for(unsigned int j = 0; j < frames2; j++){ - sumscore2 += scores[i+frames1][j+frames1]; + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + if(partition[i] == partition[j]){sumscore2 += scores[i][j];} } } - double sumscore = 0; - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - if(partition[i] == partition[j]){sumscore += scores[i][j];} + double improvement = sumscore2-sumscore1; + + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + if(scores[i][j] > 0){printf(" ");} + printf("%5.5f ",0.0001*scores[i][j]); } + printf("\n"); } - - double improvement = sumscore-sumscore1-sumscore2; - - printf("sumscore before part: %f\n",sumscore1+sumscore2); - printf("sumscore after part: %f\n",sumscore); + printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); + printf("sumscore before part: %f\n",sumscore1); + printf("sumscore after part: %f\n",sumscore2); printf("improvement: %f\n",improvement); - +//exit(0); std::vector count; for(unsigned int i = 0; i < partition.size(); i++){ - if(int(count.size()) <= partition[i]){count.resize(partition[i]+1);} + if(int(count.size()) <= partition[i]){count.resize(partition[i]+1);} count[partition[i]]++; } int minpart = count[0]; for(unsigned int i = 1; i < count.size(); i++){minpart = std::min(minpart,count[i]);} - printf("partition: ");for(unsigned int i = 0; i < partition.size(); i++){printf("%i ",int(partition[i]));}printf("\n"); - if(count.size() == 1){ + printf("points before: %i\n",model1->points.size()); model1->merge(model2,pose); + model1->recomputeModelPoints(); - model1->scores = scores; - model1->total_scores = sumscore; + printf("points after: %i\n",model1->points.size()); + //model1->scores = scores; + //model1->total_scores = sumscore; retval.updated_models.push_back(model1); retval.deleted_models.push_back(model2); - } - else if(improvement > 1){//Cannot fully fuse... separating... -// printf("Cannot fully fuse...\n"); -//exit(0); -// retval.unchanged_models.push_back(model1); -// retval.unchanged_models.push_back(model2); -// return retval; + }else if(improvement > 1){//Cannot fully fuse... separating... int c = 0; int model1_ind = partition.front(); bool model1_same = true; - for(unsigned int i = 0; i < model1->frames.size(); i++){ + for(unsigned int i = 0; i < model1->submodels.size(); i++){ if(partition[c] != model1_ind){model1_same = false;} c++; } int model2_ind = partition.back(); bool model2_same = true; - for(unsigned int i = 0; i < model2->frames.size(); i++){ + for(unsigned int i = 0; i < model2->submodels.size(); i++){ if(partition[c] != model2_ind){model2_same = false;} c++; } if(!model1_same || !model2_same){//If something changed, update models -// printf("SOMETHING CHANGED\n"); -//exit(0); - for(unsigned int i = 0; i < count.size(); i++){retval.new_models.push_back(new Model());} + for(unsigned int i = 0; i < count.size(); i++){retval.new_models.push_back(new Model());} + for(unsigned int i = 0; i < partition.size(); i++){ - retval.new_models[partition[i]]->frames.push_back(current_frames[i]); - retval.new_models[partition[i]]->relativeposes.push_back(current_poses[i]); - retval.new_models[partition[i]]->modelmasks.push_back(current_modelmasks[i]); - //retval.new_models[partition[i]]->masks.push_back(current_masks[i].clone()); - -// cv::namedWindow("mask", cv::WINDOW_AUTOSIZE); -// cv::imshow( "mask", retval.new_models[partition[i]]->masks.back()); -// cv::waitKey(0); + retval.new_models[partition[i]]->submodels.push_back(models[i]); + retval.new_models[partition[i]]->submodels_relativeposes.push_back(rps[i]); } - for(unsigned int part = 0; part < retval.new_models.size(); part++){ - Model * m = retval.new_models[part]; - m->recomputeModelPoints(); - m->scores.resize(m->frames.size()); - for(unsigned int i = 0; i < m->scores.size(); i++){ - m->scores[i].resize(m->scores.size()); - for(unsigned int j = 0; j < m->scores.size(); j++){ - m->scores[i][j] = 0; - } - } - - std::vector inds; - for(unsigned int i = 0; i < partition.size(); i++){ - if(partition[i] == int(part)){inds.push_back(i);} - } - - for(unsigned int i = 0; i < m->scores.size(); i++){ - for(unsigned int j = 0; j < m->scores.size(); j++){ - m->scores[i][j] += scores[inds[i]][inds[j]]; - } - } - - m->total_scores = 0; - for(unsigned int i = 0; i < m->scores.size(); i++){ - for(unsigned int j = 0; j < m->scores.size(); j++){ - m->total_scores += m->scores[i][j]; - } - } - -// printf("total_scores %f\n",m->total_scores); -// for(int i = 0; i < m->scores.size(); i++){ -// for(int j = 0; j < m->scores.size(); j++){ -// printf("%3.3f ",m->scores[i][j]*0.0001); -// } -// printf("\n"); -// } - - m->print(); + for(unsigned int part = 0; part < retval.new_models.size(); part++){ + retval.new_models[part]->recomputeModelPoints(); } retval.deleted_models.push_back(model1); retval.deleted_models.push_back(model2); - }else{// + }else{ retval.unchanged_models.push_back(model1); retval.unchanged_models.push_back(model2); } return retval; -// MassRegistrationPPR * massreg = new MassRegistrationPPR(0.05,false); -// massreg->setData(current_frames,current_masks); -// MassFusionResults mfr = massreg->getTransforms(current_poses); -// current_poses = mfr.poses; -// std::vector > ocs2 = getOcclusionScores(current_poses, current_frames,current_masks,current_modelmasks); -// std::vector > scores2 = getScores(ocs2); -// std::vector partition2 = getPartition(scores2,2,5,2); - -// double sumscore_after = 0; -// for(int i = 0; i < scores2.size(); i++){ -// for(int j = 0; j < scores2.size(); j++){ -// if(partition2[i] == partition2[j]){sumscore_after += scores2[i][j];} -// } -// } - -// if(sumscore_after > sumscore){ -// printf("sumscore after: %f\n",sumscore_after); - -// std::vector count2; -// for(unsigned int i = 0; i < partition2.size(); i++){ -// if(count2.size() <= partition2[i]){count2.resize(partition2[i]+1);} -// count2[partition2[i]]++; -// } - -// int minpart2 = count2[0]; -// for(unsigned int i = 1; i < count2.size(); i++){minpart2 = std::min(minpart2,count2[i]);} - -// printf("----------------------------\n"); -// printf("partition2: ");for(unsigned int i = 0; i < partition2.size(); i++){printf("%i ",partition2[i]);}printf("\n"); -// for(unsigned int i = 0; i < count2.size(); i++){printf("count2 %i -> %i\n",i,count2[i]);} - -// if(count2.size() == 1){//Mass registration solved the problem, GREAT -// printf("Mass registration solved the problem, GREAT\n"); -// for(unsigned int i = 0; i < model2->frames.size();i++){ -// model1->frames.push_back(model2->frames[i]); -// model1->masks.push_back(model2->masks[i]); -// } -// model1->relativeposes = current_poses; -// model1->recomputeModelPoints(); - -// retval.updated_models.push_back(model1); -// retval.deleted_models.push_back(model2); -// }else{//Mass registration did NOT solve the problem -// printf("Mass registration did NOT solve the problem UPDATE THIS PART\n"); - -// //Check if identical to input partition -// int c = 0; - -// int model1_ind = partition2.front(); -// bool model1_same = true; -// for(int i = 0; i < model1->frames.size(); i++){ -// if(partition2[c] != model1_ind){model1_same = false;} -// c++; -// } - -// int model2_ind = partition2.back(); -// bool model2_same = true; -// for(int i = 0; i < model2->frames.size(); i++){ -// if(partition2[c] != model2_ind){model2_same = false;} -// c++; -// } - -// if(!model1_same || !model2_same){//If something changed, update models -// printf("SOMETHING CHANGED\n"); -// retval.updated_models.push_back(model1); -// retval.updated_models.push_back(model2); -// exit(0); -// }else{// -// retval.unchanged_models.push_back(model1); -// retval.unchanged_models.push_back(model2); -// } - -// } -// } } retval.unchanged_models.push_back(model1); @@ -369,23 +269,10 @@ UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, void ModelUpdaterBasicFuse::computeMassRegistration(std::vector current_poses, std::vector current_frames,std::vector current_masks){ printf("void ModelUpdaterBasicFuse::computeMassRegistration\n"); -/* - MassRegistrationPPR * massreg = new MassRegistrationPPR(); - //massreg->viewer = viewer; - massreg->setData(current_frames,current_masks); - MassFusionResults mfr = massreg->getTransforms(current_poses); - - std::vector > ocs = getOcclusionScores(current_poses, current_frames,current_masks); - std::vector > scores = getScores(ocs,10); - std::vector partition = getPartition(scores,2,5,2); - printf("new partition: ");for(unsigned int i = 0; i < partition.size(); i++){printf("%i ",partition[i]);}printf("\n"); -*/ printf("WARNING: THIS METHOD NOT IMPLEMENTED\n"); exit(0); } -//void ModelUpdaterBasicFuse::refine(){}//No refinement behaviour added yet - void ModelUpdaterBasicFuse::setRegistration( Registration * registration_){ if(registration != 0){delete registration;} registration = registration; diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp index b30466f2..6810048e 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp @@ -3,6 +3,15 @@ namespace reglib { +bool MassRegistration::okVal(double v){return !std::isnan(v) && !(v == std::numeric_limits::infinity());} + +bool MassRegistration::isValidPoint(pcl::PointXYZRGBNormal p){ + return okVal (p.x) && okVal (p.y) && okVal (p.z) && //No nans or inf in position + okVal (p.normal_x) && okVal (p.normal_y) && okVal (p.normal_z) && //No nans or inf in normal + !(p.x == 0 && p.y == 0 && p.z == 0 ) && //not a zero point + !(p.normal_x == 0 && p.normal_y == 0 && p.normal_z == 0); //not a zero normal +} + MassRegistration::MassRegistration(){ visualizationLvl = 0; nomask = true; @@ -27,42 +36,38 @@ MassFusionResults MassRegistration::getTransforms(std::vector g return MassFusionResults(guess,0); } -void MassRegistration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){ +void MassRegistration::clearData(){} + +void MassRegistration::addModelData(Model * model, bool submodels){ + if(submodels){ + for(unsigned int i = 0; i < model->submodels.size(); i++){ + addData(model->submodels[i]->getPCLnormalcloud(1,false)); + } + }else{ + } +} + +void MassRegistration::addData(RGBDFrame* frame, ModelMask * mmask){} +void MassRegistration::addData(pcl::PointCloud::Ptr cloud){} + +void MassRegistration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){ pcl::PointCloud::Ptr scloud (new pcl::PointCloud); pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); unsigned int s_nr_data = X.cols(); unsigned int d_nr_data = Y.cols(); -//printf("nr datas: %i %i\n",s_nr_data,d_nr_data); - scloud->points.clear(); dcloud->points.clear(); for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);} for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);} - //printf("nr datas: %i %i\n",scloud->points.size(),dcloud->points.size()); - for(unsigned int i = 0; i < s_nr_data; i++){ - //if(i%100 == 0){printf("x: %f %f %f\n",X(0,i),X(1,i),X(2,i));} - } - - for(unsigned int i = 0; i < d_nr_data; i++){ - //if(i%100 == 0){printf("y: %f %f %f\n",Y(0,i),Y(1,i),Y(2,i));} - } - -viewer->removeAllPointClouds(); -viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); -viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); -viewer->spin(); - -//viewer->removeAllPointClouds(); + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); -// viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); -// viewer->spin(); -// viewer->removeAllPointClouds(); } Eigen::MatrixXd MassRegistration::getMat(int rows, int cols, double * datas){ @@ -81,6 +86,7 @@ void MassRegistration::show(std::vector Xv, bool save, std::str srand(0); for(unsigned int xi = 0; xi < Xv.size(); xi++){ + Eigen::MatrixXd X = Xv[xi]; pcl::PointCloud::Ptr cloud (new pcl::PointCloud); int r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); @@ -102,7 +108,9 @@ void MassRegistration::show(std::vector Xv, bool save, std::str char buf [1024]; sprintf(buf,"cloud%i",xi); viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); + } + if(!save){ viewer->spin(); }else{ diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR.cpp index 7ce5365c..91d369dd 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR.cpp @@ -21,7 +21,7 @@ MassRegistrationPPR::MassRegistrationPPR(double startreg, bool visualize){ dwf->startreg = startreg; dwf->debugg_print = false; func = dwf; - + fast_opt = true; nomask = true; @@ -43,180 +43,56 @@ MassRegistrationPPR::MassRegistrationPPR(double startreg, bool visualize){ visualizationLvl = 0; } + + Qp_arr = new double[3*maxcount+0]; + Qn_arr = new double[3*maxcount+0]; + Xp_arr = new double[3*maxcount+0]; + Xn_arr = new double[3*maxcount+0]; + rangeW_arr = new double[maxcount+0]; } MassRegistrationPPR::~MassRegistrationPPR(){ + for(unsigned int i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} + for(unsigned int i = 0; i < arraynormals.size(); i++){delete[] arraynormals[i];} + for(unsigned int i = 0; i < arraycolors.size(); i++){delete[] arraycolors[i];} + for(unsigned int i = 0; i < arrayinformations.size(); i++){delete[] arrayinformations[i];} + for(unsigned int i = 0; i < trees3d.size(); i++){delete trees3d[i];} for(unsigned int i = 0; i < a3dv.size(); i++){delete a3dv[i];} delete func; -} - -//double getTime(){ -// struct timeval start1; -// gettimeofday(&start1, NULL); -// return double(start1.tv_sec+(start1.tv_usec/1000000.0)); -//} - -void point_to_plane3( Eigen::Matrix3Xd & X, - Eigen::Matrix3Xd & Xn, - Eigen::Matrix3Xd & Y, - Eigen::Matrix3Xd & Yn, - Eigen::VectorXd & W){ - typedef Eigen::Matrix Vector6d; - typedef Eigen::Matrix Matrix6d; - - Matrix6d ATA; - Vector6d ATb; - ATA.setZero (); - ATb.setZero (); - - Matrix6d ATA2; - Vector6d ATb2; - ATA2.setZero (); - ATb2.setZero (); - - double dsum = 0; - unsigned int xcols = X.cols(); - for(unsigned int i=0; i < xcols; i++) { - const double & sx = X(0,i); - const double & sy = X(1,i); - const double & sz = X(2,i); - const double & dx = Y(0,i); - const double & dy = Y(1,i); - const double & dz = Y(2,i); - const double & nx = Xn(0,i); - const double & ny = Xn(1,i); - const double & nz = Xn(2,i); - const double & weight = W(i); - - double a = nz*sy - ny*sz; - double b = nx*sz - nz*sx; - double c = ny*sx - nx*sy; - - ATA.coeffRef (0) += weight * a * a; - ATA.coeffRef (1) += weight * a * b; - ATA.coeffRef (2) += weight * a * c; - ATA.coeffRef (3) += weight * a * nx; - ATA.coeffRef (4) += weight * a * ny; - ATA.coeffRef (5) += weight * a * nz; - ATA.coeffRef (7) += weight * b * b; - ATA.coeffRef (8) += weight * b * c; - ATA.coeffRef (9) += weight * b * nx; - ATA.coeffRef (10) += weight * b * ny; - ATA.coeffRef (11) += weight * b * nz; - ATA.coeffRef (14) += weight * c * c; - ATA.coeffRef (15) += weight * c * nx; - ATA.coeffRef (16) += weight * c * ny; - ATA.coeffRef (17) += weight * c * nz; - ATA.coeffRef (21) += weight * nx * nx; - ATA.coeffRef (22) += weight * nx * ny; - ATA.coeffRef (23) += weight * nx * nz; - ATA.coeffRef (28) += weight * ny * ny; - ATA.coeffRef (29) += weight * ny * nz; - ATA.coeffRef (35) += weight * nz * nz; - - double d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz); - dsum += d; - ATb.coeffRef (0) += a * d; - ATb.coeffRef (1) += b * d; - ATb.coeffRef (2) += c * d; - ATb.coeffRef (3) += nx * d; - ATb.coeffRef (4) += ny * d; - ATb.coeffRef (5) += nz * d; - } - - for(unsigned int i=0; i < xcols; i++) { - const double & sx = Y(0,i); - const double & sy = Y(1,i); - const double & sz = Y(2,i); - const double & dx = X(0,i); - const double & dy = X(1,i); - const double & dz = X(2,i); - const double & nx = Yn(0,i); - const double & ny = Yn(1,i); - const double & nz = Yn(2,i); - const double & weight = W(i); - - double a = nz*sy - ny*sz; - double b = nx*sz - nz*sx; - double c = ny*sx - nx*sy; - - ATA.coeffRef (0) += weight * a * a; - ATA.coeffRef (1) += weight * a * b; - ATA.coeffRef (2) += weight * a * c; - ATA.coeffRef (3) += weight * a * nx; - ATA.coeffRef (4) += weight * a * ny; - ATA.coeffRef (5) += weight * a * nz; - ATA.coeffRef (7) += weight * b * b; - ATA.coeffRef (8) += weight * b * c; - ATA.coeffRef (9) += weight * b * nx; - ATA.coeffRef (10) += weight * b * ny; - ATA.coeffRef (11) += weight * b * nz; - ATA.coeffRef (14) += weight * c * c; - ATA.coeffRef (15) += weight * c * nx; - ATA.coeffRef (16) += weight * c * ny; - ATA.coeffRef (17) += weight * c * nz; - ATA.coeffRef (21) += weight * nx * nx; - ATA.coeffRef (22) += weight * nx * ny; - ATA.coeffRef (23) += weight * nx * nz; - ATA.coeffRef (28) += weight * ny * ny; - ATA.coeffRef (29) += weight * ny * nz; - ATA.coeffRef (35) += weight * nz * nz; - - double d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz); - dsum += d; - ATb.coeffRef (0) -= a * d; - ATb.coeffRef (1) -= b * d; - ATb.coeffRef (2) -= c * d; - ATb.coeffRef (3) -= nx * d; - ATb.coeffRef (4) -= ny * d; - ATb.coeffRef (5) -= nz * d; - } + delete[] Qp_arr; + delete[] Qn_arr; + delete[] Xp_arr; + delete[] Xn_arr; + delete[] rangeW_arr; +} +void MassRegistrationPPR::addModelData(Model * model_, bool submodels){ + //model = model_; + printf("addModelData\n"); - ATA.coeffRef (6) = ATA.coeff (1); - ATA.coeffRef (12) = ATA.coeff (2); - ATA.coeffRef (13) = ATA.coeff (8); - ATA.coeffRef (18) = ATA.coeff (3); - ATA.coeffRef (19) = ATA.coeff (9); - ATA.coeffRef (20) = ATA.coeff (15); - ATA.coeffRef (24) = ATA.coeff (4); - ATA.coeffRef (25) = ATA.coeff (10); - ATA.coeffRef (26) = ATA.coeff (16); - ATA.coeffRef (27) = ATA.coeff (22); - ATA.coeffRef (30) = ATA.coeff (5); - ATA.coeffRef (31) = ATA.coeff (11); - ATA.coeffRef (32) = ATA.coeff (17); - ATA.coeffRef (33) = ATA.coeff (23); - ATA.coeffRef (34) = ATA.coeff (29); - - // Solve A*x = b - Vector6d x = static_cast (ATA.inverse () * ATb); - Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0))); - - X = transformation*X; - transformation(0,3) = 0; - transformation(1,3) = 0; - transformation(2,3) = 0; - Xn = transformation*Xn; - //std::cout << x << std::endl; - - //// Construct the transformation matrix from x - //constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix); -} + if(submodels){ + for(unsigned int i = 0; i < model_->submodels.size(); i++){ + addData(model_->submodels[i]->getPCLnormalcloud(1,false)); + } + }else{ + //setData(model_->frames,model_->modelmasks); -bool okVal(double v){return !std::isnan(v) && !(v == std::numeric_limits::infinity());} +// for(unsigned int i = 0; i < model->submodels.size(); i++){ +// addData(model->submodels[i]->getPCLnormalcloud(1,false)); +// } -bool isValidPoint(pcl::PointXYZRGBNormal p){ - return !okVal (p.x) && !okVal (p.y) && !okVal (p.z) && //No nans or inf in position - !okVal (p.normal_x) && !okVal (p.normal_y) && !okVal (p.normal_z) && //No nans or inf in normal - !(p.x == 0 && p.y == 0 && p.z == 0 ) && //not a zero point - !(p.normal_x == 0 && p.normal_y == 0 && p.normal_z == 0); //not a zero normal + unsigned int nr_frames = model_->frames.size(); + for(unsigned int i = 0; i < nr_frames; i++){ + addData(model_->frames[i], model_->modelmasks[i]); + } + } } void MassRegistrationPPR::setData(std::vector< pcl::PointCloud::Ptr > all_clouds){ + double total_load_time_start = getTime(); unsigned int nr_frames = all_clouds.size(); if(arraypoints.size() > 0){ @@ -248,19 +124,22 @@ void MassRegistrationPPR::setData(std::vector< pcl::PointCloud::Ptr cloud = all_clouds[i]; int count = 0; for(unsigned int i = 0; i < cloud->points.size(); i++){ - if (isValidPoint(cloud->points[i])){ - count++; - } + if (isValidPoint(cloud->points[i])){count++;} } if(count < 10){ @@ -271,6 +150,10 @@ void MassRegistrationPPR::setData(std::vector< pcl::PointCloud & X = points[i]; Eigen::Matrix & C = colors[i]; Eigen::Matrix & Xn = normals[i]; - Eigen::Matrix & tX = transformed_points[i]; - Eigen::Matrix & tXn = transformed_normals[i]; + Eigen::VectorXd information (count); int c = 0; @@ -306,6 +192,16 @@ void MassRegistrationPPR::setData(std::vector< pcl::PointCloudbuildIndex(); } + + //printf("total load time: %5.5f\n",getTime()-total_load_time_start); } void MassRegistrationPPR::setData(std::vector frames_,std::vector mmasks_){ - frames = frames_; - mmasks = mmasks_; + double total_load_time_start = getTime(); + // frames = frames_; + // mmasks = mmasks_; - unsigned int nr_frames = frames.size(); + unsigned int nr_frames = frames_.size(); if(arraypoints.size() > 0){ for(unsigned int i = 0; i < arraypoints.size(); i++){ @@ -357,6 +256,17 @@ void MassRegistrationPPR::setData(std::vector frames_,std::vector frames_,std::vectormaskvec; unsigned char * rgbdata = (unsigned char *)(frames[i]->rgb.data); unsigned short * depthdata = (unsigned short *)(frames[i]->depth.data); float * normalsdata = (float *)(frames[i]->normals.data); -// Eigen::Matrix4d p = poses[i]; -// float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); -// float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); -// float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - Camera * camera = frames[i]->camera; const unsigned int width = camera->width; const unsigned int height = camera->height; @@ -394,24 +305,11 @@ void MassRegistrationPPR::setData(std::vector frames_,std::vectorfx; const float ify = 1.0/camera->fy; - - int count1 = 0; - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height; h++){ - int ind = h*width+w; - if(maskvec[ind]){ - float z = idepth*float(depthdata[ind]); - float xn = normalsdata[3*ind+0]; - if(z > 0.2 && xn != 2){count1++;} - } - } - } - int count = 0; for(unsigned int w = 0; w < width; w+=maskstep){ for(unsigned int h = 0; h < height; h+=maskstep){ int ind = h*width+w; - if(maskvec[ind]){ + if(maskvec[ind] || nomask){ float z = idepth*float(depthdata[ind]); float xn = normalsdata[3*ind+0]; if(z > 0.2 && xn != 2){count++;} @@ -419,6 +317,8 @@ void MassRegistrationPPR::setData(std::vector frames_,std::vector frames_,std::vector frames_,std::vector & X = points[i]; Eigen::Matrix & C = colors[i]; @@ -450,7 +358,7 @@ void MassRegistrationPPR::setData(std::vector frames_,std::vector frames_,std::vector frames_,std::vector frames_,std::vectorbuildIndex(); } +*/ + //printf("total load time: %5.5f\n",getTime()-total_load_time_start); +} + +void MassRegistrationPPR::rematch(std::vector poses, std::vector prev_poses, bool first){ + //printf("rematch\n"); + double new_good_rematches = 0; + double new_total_rematches = 0; + unsigned int nr_frames = poses.size(); + + int rmt = 2; + + if(rmt==2){ + int ignores = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + nr_matches[i] = 0; + + double * ap = arraypoints[i]; + const unsigned int nr_ap = nr_arraypoints[i]; + + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + if(!first){ + Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); + + Eigen::Affine3d diff = prev_rp.inverse()*rp; + + double change_trans = 0; + double change_rot = 0; + double dt = 0; + for(unsigned int k = 0; k < 3; k++){ + dt += diff(k,3)*diff(k,3); + for(unsigned int l = 0; l < 3; l++){ + if(k == l){ change_rot += fabs(1-diff(k,l));} + else{ change_rot += fabs(diff(k,l));} + } + } + change_trans += sqrt(dt); + + //printf("%f %f\n",change_trans,change_rot); + + if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;} + } + + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + std::vector & matchid = matchids[i][j]; + matchid.resize(nr_ap); + Tree3d * t3d = trees3d[j]; + + #pragma omp parallel for num_threads(8) + for(unsigned int k = 0; k < nr_ap; ++k) { + int prev = matchid[k]; + double qp [3]; + const double & src_x = ap[3*k+0]; + const double & src_y = ap[3*k+1]; + const double & src_z = ap[3*k+2]; + + qp[0] = m00*src_x + m01*src_y + m02*src_z + m03; + qp[1] = m10*src_x + m11*src_y + m12*src_z + m13; + qp[2] = m20*src_x + m21*src_y + m22*src_z + m23; + + size_t ret_index; double out_dist_sqr; + nanoflann::KNNResultSet resultSet(1); + resultSet.init(&ret_index, &out_dist_sqr ); + t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + + int current = ret_index; + new_good_rematches += prev != current; + new_total_rematches++; + matchid[k] = current; + } + nr_matches[i] += matchid.size(); + } + } + //printf("ignores: %i rematches: %i\n",ignores,nr_frames*(nr_frames-1)-ignores); + } + + if(rmt==1){ + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + nr_matches[i] = 0; + + double * ap = arraypoints[i]; + const unsigned int nr_ap = nr_arraypoints[i]; + + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + std::vector & matchid = matchids[i][j]; + matchid.resize(nr_ap); + Tree3d * t3d = trees3d[j]; + + for(unsigned int k = 0; k < nr_ap; ++k) { + int prev = matchid[k]; + double qp [3]; + const double & src_x = ap[3*k+0]; + const double & src_y = ap[3*k+1]; + const double & src_z = ap[3*k+2]; + + qp[0] = m00*src_x + m01*src_y + m02*src_z + m03; + qp[1] = m10*src_x + m11*src_y + m12*src_z + m13; + qp[2] = m20*src_x + m21*src_y + m22*src_z + m23; + + size_t ret_index; double out_dist_sqr; + nanoflann::KNNResultSet resultSet(1); + resultSet.init(&ret_index, &out_dist_sqr ); + t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + + int current = ret_index; + new_good_rematches += prev != current; + new_total_rematches++; + matchid[k] = current; + } + nr_matches[i] += matchid.size(); + } + } + } + if(rmt==0){ + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + nr_matches[i] = 0; + + double * ap = arraypoints[i]; + int nr_ap = nr_arraypoints[i]; + + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + Eigen::Matrix tX = rp*points[i]; + + unsigned int nr_data = nr_datas[i]; + std::vector & matchid = matchids[i][j]; + matchid.resize(nr_data); + Tree3d * t3d = trees3d[j]; + + for(unsigned int k = 0; k < nr_data; ++k) { + int prev = matchid[k]; + double * qp = tX.col(k).data(); + + size_t ret_index; double out_dist_sqr; + nanoflann::KNNResultSet resultSet(1); + resultSet.init(&ret_index, &out_dist_sqr ); + t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + + int current = ret_index; + new_good_rematches += prev != current; + new_total_rematches++; + matchid[k] = current; + } + nr_matches[i] += matchid.size(); + } + } + } + // good_rematches += new_good_rematches; + // total_rematches += new_total_rematches +} + + +Eigen::MatrixXd MassRegistrationPPR::getAllResiduals(std::vector poses){ + unsigned int nr_frames = poses.size(); + Eigen::MatrixXd all_residuals; + + int total_matches = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + total_matches += matchids[i][j].size(); + } + } + + int all_residuals_type = 1; + + if(all_residuals_type == 1){ + switch(type) { + case PointToPoint: {all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);}break; + case PointToPlane: {all_residuals = Eigen::MatrixXd::Zero(1,total_matches);}break; + default: {printf("type not set\n");} break; + } + + int count = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + + double * api = arraypoints[i]; + double * ani = arraynormals[i]; + double * aci = arraycolors[i]; + double * aii = arrayinformations[i]; + const unsigned int nr_api = nr_arraypoints[i]; + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + + double * apj = arraypoints[j]; + double * anj = arraynormals[j]; + double * acj = arraycolors[j]; + double * aij = arrayinformations[j]; + const unsigned int nr_apj = nr_arraypoints[j]; + + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + if(type == PointToPlane){ + for(unsigned int ki = 0; ki < nr_api; ++ki) { + int kj = matchidi[ki]; + if( kj < 0 || kj >= nr_apj){continue;} + const double & src_x = api[3*ki+0]; + const double & src_y = api[3*ki+1]; + const double & src_z = api[3*ki+2]; + + const double & src_nx = ani[3*ki+0]; + const double & src_ny = ani[3*ki+1]; + const double & src_nz = ani[3*ki+2]; + + const double & info_i = aii[ki]; + + const double & dst_x = apj[3*kj+0]; + const double & dst_y = apj[3*kj+1]; + const double & dst_z = apj[3*kj+2]; + + + const double & info_j = aij[kj]; + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + float txn = m00*src_nx + m01*src_ny + m02*src_nz; + float tyn = m10*src_nx + m11*src_ny + m12*src_nz; + float tzn = m20*src_nx + m21*src_ny + m22*src_nz; + + const double rw = 1.0/(1.0/info_i+1.0/info_j); + const double di = (txn*(tx-dst_x) + tyn*(ty-dst_y) + tzn*(tz-dst_z))*rw; + all_residuals(0,count) = di; + count++; + } + } + + if(type == PointToPoint){ + for(unsigned int ki = 0; ki < nr_api; ++ki) { + int kj = matchidi[ki]; + if( kj < 0 ){continue;} + const double & src_x = api[3*ki+0]; + const double & src_y = api[3*ki+1]; + const double & src_z = api[3*ki+2]; + + const double & info_i = aii[ki]; + + const double & dst_x = apj[3*kj+0]; + const double & dst_y = apj[3*kj+1]; + const double & dst_z = apj[3*kj+2]; + + const double & info_j = aij[kj]; + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + const double rw = 1.0/(1.0/info_i+1.0/info_j); + all_residuals(0,count) = (tx-dst_x)*rw; + all_residuals(1,count) = (ty-dst_y)*rw; + all_residuals(2,count) = (tz-dst_z)*rw; + count++; + } + } + } + } + } + + if(all_residuals_type == 0){ + switch(type) { + case PointToPoint: {all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);}break; + case PointToPlane: {all_residuals = Eigen::MatrixXd::Zero(1,total_matches);}break; + default: {printf("type not set\n");} break; + } + + int count = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + Eigen::Matrix & tXi = transformed_points[i]; + Eigen::Matrix & tXni = transformed_normals[i]; + Eigen::VectorXd & informationi = informations[i]; + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + Eigen::Matrix & tXj = transformed_points[j]; + Eigen::Matrix & tXnj = transformed_normals[j]; + Eigen::VectorXd & informationj = informations[j]; + Eigen::Matrix3Xd Xp = Eigen::Matrix3Xd::Zero(3, matchesi); + Eigen::Matrix3Xd Xn = Eigen::Matrix3Xd::Zero(3, matchesi); + Eigen::Matrix3Xd Qp = Eigen::Matrix3Xd::Zero(3, matchesi); + Eigen::Matrix3Xd Qn = Eigen::Matrix3Xd::Zero(3, matchesi); + Eigen::VectorXd rangeW = Eigen::VectorXd::Zero( matchesi); + + for(unsigned int ki = 0; ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( ki >= Qp.cols() || kj < 0 || kj >= tXj.cols() ){continue;} + Qp.col(ki) = tXj.col(kj); + Qn.col(ki) = tXnj.col(kj); + Xp.col(ki) = tXi.col(ki); + Xn.col(ki) = tXni.col(ki); + rangeW(ki) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj)); + } + Eigen::MatrixXd residuals; + switch(type) { + case PointToPoint: {residuals = Xp-Qp;} break; + case PointToPlane: { + residuals = Eigen::MatrixXd::Zero(1, Xp.cols()); + for(int i=0; i >() ); + nr_datas.push_back( 0); + + points.push_back( Eigen::Matrix()); + colors.push_back( Eigen::Matrix()); + normals.push_back( Eigen::Matrix()); + transformed_points.push_back( Eigen::Matrix()); + transformed_normals.push_back( Eigen::Matrix()); + + informations.push_back( Eigen::VectorXd()); + + nr_arraypoints.push_back(0); + + arraypoints.push_back(0); + arraynormals.push_back(0); + arraycolors.push_back(0); + arrayinformations.push_back(0); + + trees3d.push_back(0); + a3dv.push_back(0); + is_ok.push_back(false); + + unsigned int i = frames.size()-1; + //printf("loading data for %i, nomask %i\n",i,nomask); + bool * maskvec = mmasks[i]->maskvec; + unsigned char * rgbdata = (unsigned char *)(frames[i]->rgb.data); + unsigned short * depthdata = (unsigned short *)(frames[i]->depth.data); + float * normalsdata = (float *)(frames[i]->normals.data); + + Camera * camera = frames[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + int count = 0; + for(unsigned int w = 0; w < width; w+=maskstep){ + for(unsigned int h = 0; h < height; h+=maskstep){ + int ind = h*width+w; + if(maskvec[ind] || nomask){ + float z = idepth*float(depthdata[ind]); + float xn = normalsdata[3*ind+0]; + if(z > 0.2 && xn != 2){count++;} + } + } + } + +// printf("count: %i\n",count); + + if(count < 10){ + is_ok[i] = false; + return; + }else{ + is_ok[i] = true; + } + + double * ap = new double[3*count]; + double * an = new double[3*count]; + double * ac = new double[3*count]; + double * ai = new double[3*count]; + + nr_datas[i] = count; + //matchids[i].resize(nr_frames); + points[i].resize(Eigen::NoChange,count); + colors[i].resize(Eigen::NoChange,count); + normals[i].resize(Eigen::NoChange,count); + transformed_points[i].resize(Eigen::NoChange,count); + transformed_normals[i].resize(Eigen::NoChange,count); + + nr_arraypoints[i] = count; + + arraypoints[i] = ap; + arraynormals[i] = an; + arraycolors[i] = ac; + arrayinformations[i] = ai; + + Eigen::Matrix & X = points[i]; + Eigen::Matrix & C = colors[i]; + Eigen::Matrix & Xn = normals[i]; + Eigen::Matrix & tX = transformed_points[i]; + Eigen::Matrix & tXn = transformed_normals[i]; + Eigen::VectorXd information (count); + + int c = 0; + for(unsigned int w = 0; w < width; w+=maskstep){ + for(unsigned int h = 0; h < height;h+=maskstep){ + if(c == count){continue;} + int ind = h*width+w; + if(maskvec[ind] || nomask){ + float z = idepth*float(depthdata[ind]); + float xn = normalsdata[3*ind+0]; + + if(z > 0.2 && xn != 2){ + float yn = normalsdata[3*ind+1]; + float zn = normalsdata[3*ind+2]; + + float x = (w - cx) * z * ifx; + float y = (h - cy) * z * ify; + + ap[3*c+0] =x; + ap[3*c+1] =y; + ap[3*c+2] =z; + + an[3*c+0] =xn; + an[3*c+1] =yn; + an[3*c+2] =zn; + + ac[3*c+0] =rgbdata[3*ind+2]; + ac[3*c+1] =rgbdata[3*ind+1]; + ac[3*c+2] =rgbdata[3*ind+0]; + + ai[c] = pow(fabs(z),-2);//1.0/(z*z); + + X(0,c) = x; + X(1,c) = y; + X(2,c) = z; + Xn(0,c) = xn; + Xn(1,c) = yn; + Xn(2,c) = zn; + + information(c) = ai[c];//1.0/(z*z); + C(0,c) = rgbdata[3*ind+0]; + C(1,c) = rgbdata[3*ind+1]; + C(2,c) = rgbdata[3*ind+2]; + c++; + } + } + } + } + informations[i] = information; + + ArrayData3D * a3d = new ArrayData3D; + a3d->data = ap; + a3d->rows = count; + a3dv[i] = a3d; + trees3d[i] = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + trees3d[i]->buildIndex(); + //printf("total load time: %5.5f\n",getTime()-total_load_time_start); +} + +void MassRegistrationPPR::addData(pcl::PointCloud::Ptr cloud){ + double total_load_time_start = getTime(); + + nr_matches.push_back( 0); + matchids.push_back( std::vector< std::vector >() ); + nr_datas.push_back( 0); + + points.push_back( Eigen::Matrix()); + colors.push_back( Eigen::Matrix()); + normals.push_back( Eigen::Matrix()); + transformed_points.push_back( Eigen::Matrix()); + transformed_normals.push_back( Eigen::Matrix()); + + informations.push_back( Eigen::VectorXd()); + + nr_arraypoints.push_back(0); + + arraypoints.push_back(0); + arraynormals.push_back(0); + arraycolors.push_back(0); + arrayinformations.push_back(0); + + trees3d.push_back(0); + a3dv.push_back(0); + is_ok.push_back(false); + + unsigned int i = points.size()-1; + + int count = 0; + for(unsigned int j = 0; j < cloud->points.size(); j+=maskstep){ + if (isValidPoint(cloud->points[j])){count++;} + } + + printf("count: %i\n",count); + + if(count < 10){ + is_ok[i] = false; + return; + }else{ + is_ok[i] = true; + } + + double * ap = new double[3*count]; + double * an = new double[3*count]; + double * ac = new double[3*count]; + double * ai = new double[3*count]; + + nr_datas[i] = count; + points[i].resize(Eigen::NoChange,count); + colors[i].resize(Eigen::NoChange,count); + normals[i].resize(Eigen::NoChange,count); + transformed_points[i].resize(Eigen::NoChange,count); + transformed_normals[i].resize(Eigen::NoChange,count); + + nr_arraypoints[i] = count; + + arraypoints[i] = ap; + arraynormals[i] = an; + arraycolors[i] = ac; + arrayinformations[i] = ai; + + Eigen::Matrix & X = points[i]; + Eigen::Matrix & C = colors[i]; + Eigen::Matrix & Xn = normals[i]; + + Eigen::VectorXd information (count); + + int c = 0; + for(unsigned int j = 0; j < cloud->points.size(); j+=maskstep){ + pcl::PointXYZRGBNormal p = cloud->points[j]; + if (isValidPoint(p)){ + + float xn = p.normal_x; + float yn = p.normal_y; + float zn = p.normal_z; + + float x = p.x; + float y = p.y; + float z = p.z; + + //if(j % 1000 == 0){printf("%i -> %f %f %f\n",j,x,y,z);} + + ap[3*c+0] =x; + ap[3*c+1] =y; + ap[3*c+2] =z; + + an[3*c+0] =xn; + an[3*c+1] =yn; + an[3*c+2] =zn; + + ac[3*c+0] =p.r; + ac[3*c+1] =p.g; + ac[3*c+2] =p.b; + + ai[c] = 1/sqrt(x*x+y*y+z*z); + + X(0,c) = x; + X(1,c) = y; + X(2,c) = z; + Xn(0,c) = xn; + Xn(1,c) = yn; + Xn(2,c) = zn; + + information(c) = ai[c];//1/sqrt(2+x*x+y*y+z*z);//1.0/(z*z); + C(0,c) = p.r; + C(1,c) = p.g; + C(2,c) = p.b; + c++; + + } + } + + informations[i] = information; + + ArrayData3D * a3d = new ArrayData3D; + a3d->data = ap; + a3d->rows = count; + a3dv[i] = a3d; + trees3d[i] = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + trees3d[i]->buildIndex(); + if(visualizationLvl > 0){ + printf("total load time: %5.5f\n",getTime()-total_load_time_start); + } +} + +std::vector MassRegistrationPPR::optimize(std::vector poses){ + bool onetoone = true; + unsigned int nr_frames = poses.size(); + Eigen::MatrixXd Xo1; + + int optt = 2; + for(int outer=0; outer < 30; ++outer) { + if(getTime()-total_time_start > timeout){break;} + + //printf("outer: %i\n",outer); + + std::vector poses2 = poses; + if(type == PointToPlane && optt == 2){ + for(unsigned int i = 0; i < nr_frames; i++){ + if(getTime()-total_time_start > timeout){break;} + if(!is_ok[i]){continue;} + + unsigned int count = 0; + double * api = arraypoints[i]; + double * ani = arraynormals[i]; + double * aci = arraycolors[i]; + double * aii = arrayinformations[i]; + const unsigned int nr_api = nr_arraypoints[i]; + + Eigen::Affine3d rpi = Eigen::Affine3d(poses[i]); + + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + + double * apj = arraypoints[j]; + double * anj = arraynormals[j]; + double * acj = arraycolors[j]; + double * aij = arrayinformations[j]; + const unsigned int nr_apj = nr_arraypoints[j]; + + std::vector & matchidj = matchids[j][i]; + unsigned int matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + + + Eigen::Affine3d rpj = Eigen::Affine3d(poses[j]); + const double & mj00 = rpj(0,0); const double & mj01 = rpj(0,1); const double & mj02 = rpj(0,2); const double & mj03 = rpj(0,3); + const double & mj10 = rpj(1,0); const double & mj11 = rpj(1,1); const double & mj12 = rpj(1,2); const double & mj13 = rpj(1,3); + const double & mj20 = rpj(2,0); const double & mj21 = rpj(2,1); const double & mj22 = rpj(2,2); const double & mj23 = rpj(2,3); + + for(unsigned int ki = 0; ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( kj == -1 ){continue;} + if( kj >= matchesj){continue;} + if(!onetoone || matchidj[kj] == ki){ + Xp_arr[3*count+0] = api[3*ki+0]; + Xp_arr[3*count+1] = api[3*ki+1]; + Xp_arr[3*count+2] = api[3*ki+2]; + + Xn_arr[3*count+0] = ani[3*ki+0]; + Xn_arr[3*count+1] = ani[3*ki+1]; + Xn_arr[3*count+2] = ani[3*ki+2]; + + const double & info_i = aii[ki]; + + const double & dst_x = apj[3*kj+0]; + const double & dst_y = apj[3*kj+1]; + const double & dst_z = apj[3*kj+2]; + + const double & dst_nx = anj[3*kj+0]; + const double & dst_ny = anj[3*kj+1]; + const double & dst_nz = anj[3*kj+2]; + + const double & info_j = aij[kj]; + + Qp_arr[3*count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03; + Qp_arr[3*count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13; + Qp_arr[3*count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23; + + Qn_arr[3*count+0] = mj00*dst_nx + mj01*dst_ny + mj02*dst_nz; + Qn_arr[3*count+1] = mj10*dst_nx + mj11*dst_ny + mj12*dst_nz; + Qn_arr[3*count+2] = mj20*dst_nx + mj21*dst_ny + mj22*dst_nz; + + rangeW_arr[count] = 1.0/(1.0/info_i+1.0/info_j); + + count++; + } + } + } + + if(count == 0){break;} + + typedef Eigen::Matrix Vector6d; + typedef Eigen::Matrix Matrix6d; + + Matrix6d ATA; + Vector6d ATb; + + //std::vector Xv; + //if(visualizationLvl == 4){for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + + + + Eigen::Affine3d p = rpi; + bool do_inner = true; + for(int inner=0; inner < 15 && do_inner; ++inner) { + + pcl::PointCloud::Ptr scloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + + ATA.setZero (); + ATb.setZero (); + const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3); + const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3); + const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3); + for(unsigned int co = 0; co < count; co++){ + const double & src_x = Xp_arr[3*co+0]; + const double & src_y = Xp_arr[3*co+1]; + const double & src_z = Xp_arr[3*co+2]; + const double & src_nx = Xn_arr[3*co+0]; + const double & src_ny = Xn_arr[3*co+1]; + const double & src_nz = Xn_arr[3*co+2]; + + const double & dx = Qp_arr[3*co+0]; + const double & dy = Qp_arr[3*co+1]; + const double & dz = Qp_arr[3*co+2]; + const double & dnx = Qn_arr[3*co+0]; + const double & dny = Qn_arr[3*co+1]; + const double & dnz = Qn_arr[3*co+2]; + + const double & rw = rangeW_arr[co]; + + const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; + const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; + const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; + + const double & nx = m00*src_nx + m01*src_ny + m02*src_nz; + const double & ny = m10*src_nx + m11*src_ny + m12*src_nz; + const double & nz = m20*src_nx + m21*src_ny + m22*src_nz; + + const double & angle = nx*dnx+ny*dny+nz*dnz; + //printf("%f\n",angle); + //if(angle < 0){exit(0);continue;} + + if(angle < 0){continue;} + + double di = rw*(nx*(sx-dx) + ny*(sy-dy) + nz*(sz-dz)); + //double weight = angle*angle*angle*angle*func->getProb(di)*rw*rw; + double prob = func->getProb(di); + double weight = prob*rw*rw; + + if(visualizationLvl == 5){ + pcl::PointXYZRGBNormal p; + p.x = sx; + p.y = sy; + p.z = sz; + p.b = 0; + p.g = 255; + p.r = 0; + scloud->points.push_back(p); + + pcl::PointXYZRGBNormal p1; + p1.x = dx; + p1.y = dy; + p1.z = dz; + p1.b = 255.0*prob; + p1.g = 255.0*prob; + p1.r = 255.0*prob; + dcloud->points.push_back(p1); + } + + const double & a = nz*sy - ny*sz; + const double & b = nx*sz - nz*sx; + const double & c = ny*sx - nx*sy; + + ATA.coeffRef (0) += weight * a * a; + ATA.coeffRef (1) += weight * a * b; + ATA.coeffRef (2) += weight * a * c; + ATA.coeffRef (3) += weight * a * nx; + ATA.coeffRef (4) += weight * a * ny; + ATA.coeffRef (5) += weight * a * nz; + ATA.coeffRef (7) += weight * b * b; + ATA.coeffRef (8) += weight * b * c; + ATA.coeffRef (9) += weight * b * nx; + ATA.coeffRef (10) += weight * b * ny; + ATA.coeffRef (11) += weight * b * nz; + ATA.coeffRef (14) += weight * c * c; + ATA.coeffRef (15) += weight * c * nx; + ATA.coeffRef (16) += weight * c * ny; + ATA.coeffRef (17) += weight * c * nz; + ATA.coeffRef (21) += weight * nx * nx; + ATA.coeffRef (22) += weight * nx * ny; + ATA.coeffRef (23) += weight * nx * nz; + ATA.coeffRef (28) += weight * ny * ny; + ATA.coeffRef (29) += weight * ny * nz; + ATA.coeffRef (35) += weight * nz * nz; + + const double & d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz); + + ATb.coeffRef (0) += a * d; + ATb.coeffRef (1) += b * d; + ATb.coeffRef (2) += c * d; + ATb.coeffRef (3) += nx * d; + ATb.coeffRef (4) += ny * d; + ATb.coeffRef (5) += nz * d; + } + + ATA.coeffRef (6) = ATA.coeff (1); + ATA.coeffRef (12) = ATA.coeff (2); + ATA.coeffRef (13) = ATA.coeff (8); + ATA.coeffRef (18) = ATA.coeff (3); + ATA.coeffRef (19) = ATA.coeff (9); + ATA.coeffRef (20) = ATA.coeff (15); + ATA.coeffRef (24) = ATA.coeff (4); + ATA.coeffRef (25) = ATA.coeff (10); + ATA.coeffRef (26) = ATA.coeff (16); + ATA.coeffRef (27) = ATA.coeff (22); + ATA.coeffRef (30) = ATA.coeff (5); + ATA.coeffRef (31) = ATA.coeff (11); + ATA.coeffRef (32) = ATA.coeff (17); + ATA.coeffRef (33) = ATA.coeff (23); + ATA.coeffRef (34) = ATA.coeff (29); + + // Solve A*x = b + Vector6d x = static_cast (ATA.inverse () * ATb); + Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0))); + p = transformation*p; + + double change_t = 0; + double change_r = 0; + for(unsigned int k = 0; k < 3; k++){ + change_t += transformation(k,3)*transformation(k,3); + for(unsigned int l = 0; l < 3; l++){ + if(k == l){ change_r += fabs(1-transformation(k,l));} + else{ change_r += fabs(transformation(k,l));} + } + } + change_t = sqrt(change_t); + + if(visualizationLvl == 5){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + } + + if(change_t < stopval && change_r < stopval){do_inner = false;} + } + + + //new_opt_time += getTime()-new_opt_start; + //std::cout << p.matrix() << std::endl; + //Eigen::Matrix4d newpose = p.matrix()*poses[i]; + poses[i] = p.matrix();//newpose; + } + }else if(type == PointToPlane && optt == 1){ + for(unsigned int i = 0; i < nr_frames; i++){ + if(getTime()-total_time_start > timeout){break;} + if(!is_ok[i]){continue;} + unsigned int count = 0; + + Eigen::Matrix & tXi = transformed_points[i]; + Eigen::Matrix & Xi = points[i]; + Eigen::Matrix & tXni = transformed_normals[i]; + Eigen::Matrix & Xni = normals[i]; + Eigen::VectorXd & informationi = informations[i]; + + + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + Eigen::Matrix & tXj = transformed_points[j]; + Eigen::Matrix & tXnj = transformed_normals[j]; + Eigen::VectorXd & informationj = informations[j]; + + std::vector & matchidj = matchids[j][i]; + unsigned int matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + for(unsigned int ki = 0; ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( kj == -1 ){continue;} + if( kj >= matchesj){continue;} + if(!onetoone || matchidj[kj] == ki){ + Qp_arr[3*count+0] = tXj(0,kj); + Qp_arr[3*count+1] = tXj(1,kj); + Qp_arr[3*count+2] = tXj(2,kj); + Qn_arr[3*count+0] = tXnj(0,kj); + Qn_arr[3*count+1] = tXnj(1,kj); + Qn_arr[3*count+2] = tXnj(2,kj); + + Xp_arr[3*count+0] = tXi(0,ki); + Xp_arr[3*count+1] = tXi(1,ki); + Xp_arr[3*count+2] = tXi(2,ki); + Xn_arr[3*count+0] = tXni(0,ki); + Xn_arr[3*count+1] = tXni(1,ki); + Xn_arr[3*count+2] = tXni(2,ki); + + rangeW_arr[count] = 1.0/(1.0/informationi(ki)+1.0/informationj(kj)); + count++; + } + } + } + + if(count == 0){break;} + + typedef Eigen::Matrix Vector6d; + typedef Eigen::Matrix Matrix6d; + + Matrix6d ATA; + Vector6d ATb; + + + Eigen::Affine3d p = Eigen::Affine3d::Identity(); + bool do_inner = true; + for(int inner=0; inner < 5 && do_inner; ++inner) { + ATA.setZero (); + ATb.setZero (); + const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3); + const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3); + const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3); + for(unsigned int co = 0; co < count; co++){ + const double & src_x = Xp_arr[3*co+0]; + const double & src_y = Xp_arr[3*co+1]; + const double & src_z = Xp_arr[3*co+2]; + const double & src_nx = Xn_arr[3*co+0]; + const double & src_ny = Xn_arr[3*co+1]; + const double & src_nz = Xn_arr[3*co+2]; + + const double & dx = Qp_arr[3*co+0]; + const double & dy = Qp_arr[3*co+1]; + const double & dz = Qp_arr[3*co+2]; + const double & dnx = Qn_arr[3*co+0]; + const double & dny = Qn_arr[3*co+1]; + const double & dnz = Qn_arr[3*co+2]; + + const double & rw = rangeW_arr[co]; + + const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; + const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; + const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; + + const double & nx = m00*src_nx + m01*src_ny + m02*src_nz; + const double & ny = m10*src_nx + m11*src_ny + m12*src_nz; + const double & nz = m20*src_nx + m21*src_ny + m22*src_nz; + + if(nx*dnx+ny*dny+nz*dnz < 0){continue;} + + double di = rw*(nx*(sx-dx) + ny*(sy-dy) + nz*(sz-dz)); + double weight = func->getProb(di)*rw*rw; + + const double & a = nz*sy - ny*sz; + const double & b = nx*sz - nz*sx; + const double & c = ny*sx - nx*sy; + + ATA.coeffRef (0) += weight * a * a; + ATA.coeffRef (1) += weight * a * b; + ATA.coeffRef (2) += weight * a * c; + ATA.coeffRef (3) += weight * a * nx; + ATA.coeffRef (4) += weight * a * ny; + ATA.coeffRef (5) += weight * a * nz; + ATA.coeffRef (7) += weight * b * b; + ATA.coeffRef (8) += weight * b * c; + ATA.coeffRef (9) += weight * b * nx; + ATA.coeffRef (10) += weight * b * ny; + ATA.coeffRef (11) += weight * b * nz; + ATA.coeffRef (14) += weight * c * c; + ATA.coeffRef (15) += weight * c * nx; + ATA.coeffRef (16) += weight * c * ny; + ATA.coeffRef (17) += weight * c * nz; + ATA.coeffRef (21) += weight * nx * nx; + ATA.coeffRef (22) += weight * nx * ny; + ATA.coeffRef (23) += weight * nx * nz; + ATA.coeffRef (28) += weight * ny * ny; + ATA.coeffRef (29) += weight * ny * nz; + ATA.coeffRef (35) += weight * nz * nz; + + const double & d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz); + + ATb.coeffRef (0) += a * d; + ATb.coeffRef (1) += b * d; + ATb.coeffRef (2) += c * d; + ATb.coeffRef (3) += nx * d; + ATb.coeffRef (4) += ny * d; + ATb.coeffRef (5) += nz * d; + } + + ATA.coeffRef (6) = ATA.coeff (1); + ATA.coeffRef (12) = ATA.coeff (2); + ATA.coeffRef (13) = ATA.coeff (8); + ATA.coeffRef (18) = ATA.coeff (3); + ATA.coeffRef (19) = ATA.coeff (9); + ATA.coeffRef (20) = ATA.coeff (15); + ATA.coeffRef (24) = ATA.coeff (4); + ATA.coeffRef (25) = ATA.coeff (10); + ATA.coeffRef (26) = ATA.coeff (16); + ATA.coeffRef (27) = ATA.coeff (22); + ATA.coeffRef (30) = ATA.coeff (5); + ATA.coeffRef (31) = ATA.coeff (11); + ATA.coeffRef (32) = ATA.coeff (17); + ATA.coeffRef (33) = ATA.coeff (23); + ATA.coeffRef (34) = ATA.coeff (29); + + // Solve A*x = b + Vector6d x = static_cast (ATA.inverse () * ATb); + Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0))); + p = transformation*p; + + double change_t = 0; + double change_r = 0; + for(unsigned int k = 0; k < 3; k++){ + change_t += transformation(k,3)*transformation(k,3); + for(unsigned int l = 0; l < 3; l++){ + if(k == l){ change_r += fabs(1-transformation(k,l));} + else{ change_r += fabs(transformation(k,l));} + } + } + change_t = sqrt(change_t); + if(change_t < stopval && change_r < stopval){do_inner = false;} + } + + //new_opt_time += getTime()-new_opt_start; + //std::cout << p.matrix() << std::endl; + Eigen::Matrix4d newpose = p.matrix()*poses[i]; + poses[i] = newpose; + } + }else{ + + for(unsigned int i = 0; i < nr_frames; i++){ + if(getTime()-total_time_start > timeout){break;} + if(!is_ok[i]){continue;} + unsigned int nr_match = 0; + { + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + std::vector & matchidj = matchids[j][i]; + unsigned int matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + for(unsigned int ki = 0; ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( kj == -1 ){continue;} + if( kj >= matchesj){continue;} + if(!onetoone || matchidj[kj] == ki){ nr_match++;} + } + } + } + + Eigen::Matrix3Xd Xp = Eigen::Matrix3Xd::Zero(3, nr_match); + Eigen::Matrix3Xd Xp_ori = Eigen::Matrix3Xd::Zero(3, nr_match); + Eigen::Matrix3Xd Xn = Eigen::Matrix3Xd::Zero(3, nr_match); + + Eigen::Matrix3Xd Qp = Eigen::Matrix3Xd::Zero(3, nr_match); + Eigen::Matrix3Xd Qn = Eigen::Matrix3Xd::Zero(3, nr_match); + Eigen::VectorXd rangeW = Eigen::VectorXd::Zero( nr_match); + + int count = 0; + + Eigen::Matrix & tXi = transformed_points[i]; + Eigen::Matrix & Xi = points[i]; + Eigen::Matrix & tXni = transformed_normals[i]; + Eigen::Matrix & Xni = normals[i]; + Eigen::VectorXd & informationi = informations[i]; + + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + Eigen::Matrix & tXj = transformed_points[j]; + Eigen::Matrix & tXnj = transformed_normals[j]; + Eigen::VectorXd & informationj = informations[j]; + + std::vector & matchidj = matchids[j][i]; + unsigned int matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + for(unsigned int ki = 0; ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( kj == -1 ){continue;} + if( kj >= matchesj){continue;} + if(!onetoone || matchidj[kj] == ki){ + Qp.col(count) = tXj.col(kj); + Qn.col(count) = tXnj.col(kj); + + Xp_ori.col(count) = Xi.col(ki); + Xp.col(count) = tXi.col(ki); + + Xn.col(count) = tXni.col(ki); + rangeW(count) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj)); + count++; + } + } + } + + if(count == 0){break;} + + //showMatches(Xp,Qp); + for(int inner=0; inner < 5; ++inner) { + Eigen::MatrixXd residuals; + switch(type) { + case PointToPoint: {residuals = Xp-Qp;} break; + case PointToPlane: { + residuals = Eigen::MatrixXd::Zero(1, Xp.cols()); + for(int i=0; igetProbs(residuals); } break; + case PointToPlane: { + W = func->getProbs(residuals); + for(int k=0; k 0.0);} + } break; + default: {printf("type not set\n");} break; + } + + W = W.array()*rangeW.array()*rangeW.array(); + Xo1 = Xp; + switch(type) { + case PointToPoint: { + //RigidMotionEstimator::point_to_point(Xp, Qp, W); + pcl::TransformationFromCorrespondences tfc1; + for(unsigned int c = 0; c < nr_match; c++){tfc1.add(Eigen::Vector3f(Xp(0,c), Xp(1,c),Xp(2,c)),Eigen::Vector3f(Qp(0,c),Qp(1,c),Qp(2,c)),W(c));} + Eigen::Affine3d rot = tfc1.getTransformation().cast(); + Xp = rot*Xp; + Xn = rot.rotation()*Xn; + } break; + case PointToPlane: { + point_to_plane2(Xp, Xn, Qp, Qn, W); + } break; + default: {printf("type not set\n"); } break; + } + + double stop1 = (Xp-Xo1).colwise().norm().maxCoeff(); + Xo1 = Xp; + if(stop1 < 0.001){break; } + } + //exit(0); + pcl::TransformationFromCorrespondences tfc; + for(unsigned int c = 0; c < nr_match; c++){tfc.add(Eigen::Vector3f(Xp_ori(0,c),Xp_ori(1,c),Xp_ori(2,c)),Eigen::Vector3f(Xp(0,c),Xp(1,c),Xp(2,c)));} + poses[i] = tfc.getTransformation().cast().matrix(); + } + } + + Eigen::Matrix4d p0inv = poses[0].inverse(); + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + poses[j] = p0inv*poses[j]; + + Eigen::Matrix & tXi = transformed_points[j]; + Eigen::Matrix & Xi = points[j]; + Eigen::Matrix & tXni = transformed_normals[j]; + Eigen::Matrix & Xni = normals[j]; + + Eigen::Matrix4d p = poses[j]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + for(int c = 0; c < Xi.cols(); c++){ + float x = Xi(0,c); + float y = Xi(1,c); + float z = Xi(2,c); + + float nx = Xni(0,c); + float ny = Xni(1,c); + float nz = Xni(2,c); + + tXi(0,c) = m00*x + m01*y + m02*z + m03; + tXi(1,c) = m10*x + m11*y + m12*z + m13; + tXi(2,c) = m20*x + m21*y + m22*z + m23; + + tXni(0,c) = m00*nx + m01*ny + m02*nz; + tXni(1,c) = m10*nx + m11*ny + m12*nz; + tXni(2,c) = m20*nx + m21*ny + m22*nz; + } + } + if(isconverged(poses, poses2, stopval, stopval)){break;} + } + return poses; } -int testcount = 0; MassFusionResults MassRegistrationPPR::getTransforms(std::vector poses){ - printf("start MassRegistrationPPR::getTransforms(std::vector poses)\n"); + if(visualizationLvl > 0){printf("start MassRegistrationPPR::getTransforms(std::vector poses)\n");} unsigned int nr_frames = informations.size(); if(poses.size() != nr_frames){ printf("ERROR: poses.size() != informations.size()\n"); return MassFusionResults(); } - bool internatldebug = testcount++ > 2; fast_opt = false; if(fast_opt){ @@ -509,7 +1645,6 @@ MassFusionResults MassRegistrationPPR::getTransforms(std::vector & X = points[i]; Eigen::Matrix & Xn = normals[i]; Eigen::Matrix & tX = transformed_points[i]; @@ -537,33 +1674,12 @@ MassFusionResults MassRegistrationPPR::getTransforms(std::vectorreset(); - Eigen::MatrixXd Xo1; - int imgcount = 0; - - double good_rematches = 0; - double total_rematches = 0; - - double good_opt = 0; - double bad_opt = 0; - - double rematch_time = 0; - double residuals_time = 0; - double computeModel_time = 0; - double setup_matches_time = 0; - double setup_equation_time = 0; - double setup_equation_time2 = 0; - double solve_equation_time = 0; - double total_time_start = getTime(); - - int savecounter = 0; - - bool onetoone = true; - char buf [1024]; if(visualizationLvl > 0){ std::vector Xv; @@ -572,797 +1688,59 @@ MassFusionResults MassRegistrationPPR::getTransforms(std::vector poses0 = poses; + for(int funcupdate=0; funcupdate < 100; ++funcupdate) { if(getTime()-total_time_start > timeout){break;} if(visualizationLvl == 2){std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} - for(int rematching=0; rematching < 10; ++rematching) { + for(int rematching=0; rematching < 20; ++rematching) { if(visualizationLvl == 3){std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} std::vector poses1 = poses; - double rematch_time_start = getTime(); - - double new_good_rematches = 0; - double new_total_rematches = 0; - for(unsigned int i = 0; i < nr_frames; i++){ - if(!is_ok[i]){continue;} - nr_matches[i] = 0; - - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - if(i == j){continue;} - Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); - Eigen::Matrix tX = rp*points[i]; - - unsigned int nr_data = nr_datas[i]; - std::vector & matchid = matchids[i][j]; - matchid.resize(nr_data); - Tree3d * t3d = trees3d[j]; - - for(unsigned int k = 0; k < nr_data; ++k) { - int prev = matchid[k]; - double * qp = tX.col(k).data(); - size_t ret_index; - double out_dist_sqr; - nanoflann::KNNResultSet resultSet(1); - resultSet.init(&ret_index, &out_dist_sqr ); - t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); - int current = ret_index; - new_good_rematches += prev != current; - new_total_rematches++; - matchid[k] = current; - } - nr_matches[i] += matchid.size(); - } - } - - good_rematches += new_good_rematches; - total_rematches += new_total_rematches; + double rematch_time_start = getTime(); + rematch(poses,poses0,first); + first = false; + poses0 = poses; rematch_time += getTime()-rematch_time_start; - // printf("rematch_time: %f\n",rematch_time); - // printf("new percentage: %5.5f (good_rematches: %f total_rematches: %f)\n",new_good_rematches/new_total_rematches,new_good_rematches,new_total_rematches); - // printf("tot percentage: %5.5f (good_rematches: %f total_rematches: %f)\n",good_rematches/total_rematches,good_rematches,total_rematches); - for(unsigned int i = 0; i < nr_frames; i++){ - if(!is_ok[i]){continue;} - nr_matches[i] = 0; - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - nr_matches[i] += matchids[i][j].size()+matchids[j][i].size(); - } - } - - int total_matches = 0; - for(unsigned int i = 0; i < nr_frames; i++){ - if(!is_ok[i]){continue;} - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - total_matches += matchids[i][j].size(); - } - } for(int lala = 0; lala < 1; lala++){ + if(visualizationLvl > 0){ + printf("funcupdate: %i rematching: %i lala: %i\n",funcupdate,rematching,lala); + printf("total_time: %5.5f\n",getTime()-total_time_start); + printf("rematch_time: %5.5f\n",rematch_time); + printf("compM residuals_time:%5.5f\n",residuals_time); + printf("computeModel: %5.5f\n",computeModel_time); + printf("opt_time: %5.5f\n",opt_time); + } if(visualizationLvl == 4){std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} std::vector poses2b = poses; - double residuals_time_start = getTime(); - Eigen::MatrixXd all_residuals; - switch(type) { - case PointToPoint: {all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);}break; - case PointToPlane: {all_residuals = Eigen::MatrixXd::Zero(1,total_matches);}break; - default: {printf("type not set\n");} break; - } - - int count = 0; - for(unsigned int i = 0; i < nr_frames; i++){ - if(!is_ok[i]){continue;} - Eigen::Matrix & tXi = transformed_points[i]; - Eigen::Matrix & tXni = transformed_normals[i]; - Eigen::VectorXd & informationi = informations[i]; - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - if(i == j){continue;} - std::vector & matchidi = matchids[i][j]; - unsigned int matchesi = matchidi.size(); - Eigen::Matrix & tXj = transformed_points[j]; - Eigen::Matrix & tXnj = transformed_normals[j]; - Eigen::VectorXd & informationj = informations[j]; - Eigen::Matrix3Xd Xp = Eigen::Matrix3Xd::Zero(3, matchesi); - Eigen::Matrix3Xd Xn = Eigen::Matrix3Xd::Zero(3, matchesi); - Eigen::Matrix3Xd Qp = Eigen::Matrix3Xd::Zero(3, matchesi); - Eigen::Matrix3Xd Qn = Eigen::Matrix3Xd::Zero(3, matchesi); - Eigen::VectorXd rangeW = Eigen::VectorXd::Zero( matchesi); - for(unsigned int ki = 0; ki < matchesi; ki++){ - int kj = matchidi[ki]; - if( ki >= Qp.cols() || kj < 0 || kj >= tXj.cols() ){continue;} - Qp.col(ki) = tXj.col(kj); - Qn.col(ki) = tXnj.col(kj); - Xp.col(ki) = tXi.col(ki); - Xn.col(ki) = tXni.col(ki); - rangeW(ki) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj)); - } - Eigen::MatrixXd residuals; - switch(type) { - case PointToPoint: {residuals = Xp-Qp;} break; - case PointToPlane: { - residuals = Eigen::MatrixXd::Zero(1, Xp.cols()); - for(int i=0; icomputeModel(all_residuals); computeModel_time += getTime()-computeModel_time_start; - if(fast_opt){ - double setup_matches_time_start = getTime(); - - std::vector< Eigen::Matrix4d > localposes = poses; - std::vector< std::vector < std::vector< std::pair > > > current_matches; - std::vector< std::vector < std::vector< double > > > rangeW; - - current_matches.resize(nr_frames); - rangeW.resize(nr_frames); - for(unsigned int i = 0; i < nr_frames; i++){ - current_matches[i].resize(nr_frames); - rangeW[i].resize(nr_frames); - if(!is_ok[i]){continue;} - - Eigen::VectorXd & informationi = informations[i]; - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - if(i == j){continue;} - Eigen::VectorXd & informationj = informations[j]; - - std::vector & matchidj = matchids[j][i]; - unsigned int matchesj = matchidj.size(); - std::vector & matchidi = matchids[i][j]; - unsigned int matchesi = matchidi.size(); - - std::vector > & cm = current_matches[i][j]; - std::vector & rw = rangeW[i][j]; - - for(unsigned int ki = 0; ki < matchesi; ki++){ - int kj = matchidi[ki]; - if( kj < 0 || kj >= matchesj){continue;} //Make sure that failed searches dont screw things up - if(matchidj[kj] != ki){continue;}//Only 1-to-1 matching - - cm.push_back(std::make_pair(ki,kj)); - rw.push_back(1.0/(1.0/informationi(ki)+1.0/informationj(kj))); - } - } - } - - setup_matches_time += getTime()-setup_matches_time_start; - - typedef Eigen::Matrix Vector6d; - typedef Eigen::Matrix Matrix6d; + double opt_time_start = getTime(); + poses = optimize(poses); + opt_time += getTime()-opt_time_start; - std::vector> A; - std::vector> b; - A.resize(nr_frames); - b.resize(nr_frames); - for(unsigned int i = 0; i < nr_frames; i++){ - A[i].resize(nr_frames); - b[i].resize(nr_frames); - } - - std::vector> A2; - std::vector> b2; - A2.resize(nr_frames); - b2.resize(nr_frames); - for(unsigned int i = 0; i < nr_frames; i++){ - A2[i].resize(nr_frames); - b2[i].resize(nr_frames); - for(unsigned int j = 0; j < nr_frames; j++){ - Matrix6d & ATA = A[i][j]; - Vector6d & ATb = b[i][j]; - ATA.setZero (); - ATb.setZero (); - } - } - - for(int iteration = 0; iteration < 5; iteration++){ - printf("iteration: %i\n",iteration); - - double total_score = 0; - - double setup_equation_time_start = getTime(); - for(unsigned int i = 0; i < nr_frames; i++){ - if(!is_ok[i]){continue;} - Eigen::Matrix & tXi = transformed_points[i]; - Eigen::Matrix & tXni = transformed_normals[i]; - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - if(i == j){continue;} - Eigen::Matrix & tXj = transformed_points[j]; - Eigen::Matrix & tXnj = transformed_normals[j]; - - std::vector > & cm = current_matches[i][j]; - std::vector & rw = rangeW[i][j]; - unsigned int current_nr_matches = cm.size(); - - Matrix6d & ATA = A[i][j]; - Vector6d & ATb = b[i][j]; - ATA.setZero (); - ATb.setZero (); - - //Matches from ki to kj - for(unsigned int k = 0; k < current_nr_matches; k++){ - unsigned int ki = cm[k].first; - unsigned int kj = cm[k].second; - double rwij = rw[k]; - - const float & sx = tXi(0,ki); - const float & sy = tXi(1,ki); - const float & sz = tXi(2,ki); - - const float & dx = tXj(0,kj); - const float & dy = tXj(1,kj); - const float & dz = tXj(2,kj); - - const float & nx = tXnj(0,kj); - const float & ny = tXnj(1,kj); - const float & nz = tXnj(2,kj); - - - double a = nz*sy - ny*sz; - double b = nx*sz - nz*sx; - double c = ny*sx - nx*sy; - - // 0 1 2 3 4 5 - // 6 7 8 9 10 11 - // 12 13 14 15 16 17 - // 18 19 20 21 22 23 - // 24 25 26 27 28 29 - // 30 31 32 33 34 35 - - ATA.coeffRef (0) += a * a; - ATA.coeffRef (1) += a * b; - ATA.coeffRef (2) += a * c; - ATA.coeffRef (3) += a * nx; - ATA.coeffRef (4) += a * ny; - ATA.coeffRef (5) += a * nz; - ATA.coeffRef (7) += b * b; - ATA.coeffRef (8) += b * c; - ATA.coeffRef (9) += b * nx; - ATA.coeffRef (10) += b * ny; - ATA.coeffRef (11) += b * nz; - ATA.coeffRef (14) += c * c; - ATA.coeffRef (15) += c * nx; - ATA.coeffRef (16) += c * ny; - ATA.coeffRef (17) += c * nz; - ATA.coeffRef (21) += nx * nx; - ATA.coeffRef (22) += nx * ny; - ATA.coeffRef (23) += nx * nz; - ATA.coeffRef (28) += ny * ny; - ATA.coeffRef (29) += ny * nz; - ATA.coeffRef (35) += nz * nz; - - double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz; - total_score += d*d; - ATb.coeffRef (0) += a * d; - ATb.coeffRef (1) += b * d; - ATb.coeffRef (2) += c * d; - ATb.coeffRef (3) += nx * d; - ATb.coeffRef (4) += ny * d; - ATb.coeffRef (5) += nz * d; - - } - ATA.coeffRef (6) = ATA.coeff (1); - ATA.coeffRef (12) = ATA.coeff (2); - ATA.coeffRef (13) = ATA.coeff (8); - ATA.coeffRef (18) = ATA.coeff (3); - ATA.coeffRef (19) = ATA.coeff (9); - ATA.coeffRef (20) = ATA.coeff (15); - ATA.coeffRef (24) = ATA.coeff (4); - ATA.coeffRef (25) = ATA.coeff (10); - ATA.coeffRef (26) = ATA.coeff (16); - ATA.coeffRef (27) = ATA.coeff (22); - ATA.coeffRef (30) = ATA.coeff (5); - ATA.coeffRef (31) = ATA.coeff (11); - ATA.coeffRef (32) = ATA.coeff (17); - ATA.coeffRef (33) = ATA.coeff (23); - ATA.coeffRef (34) = ATA.coeff (29); - } - } - setup_equation_time += getTime()-setup_equation_time_start; - - double setup_equation_time_start2 = getTime(); - for(unsigned int i = 0; i < nr_frames; i++){ - if(!is_ok[i]){continue;} - Eigen::Matrix & tXi = transformed_points[i]; - Eigen::Matrix & tXni = transformed_normals[i]; - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - if(i == j){continue;} - Eigen::Matrix & tXj = transformed_points[j]; - Eigen::Matrix & tXnj = transformed_normals[j]; - - std::vector > & cm = current_matches[i][j]; - std::vector & rw = rangeW[i][j]; - unsigned int current_nr_matches = cm.size(); - - Matrix6d & ATA = A2[i][j]; - Vector6d & ATb = b2[i][j]; - ATA.setZero (); - ATb.setZero (); - - //Matches from ki to kj - for(unsigned int k = 0; k < current_nr_matches; k++){ - unsigned int ki = cm[k].first; - unsigned int kj = cm[k].second; - double rwij = rw[k]; - - - - const float & sx = tXi(0,ki); - const float & sy = tXi(1,ki); - const float & sz = tXi(2,ki); - - const float & snx = tXni(0,ki); - const float & sny = tXni(1,ki); - const float & snz = tXni(2,ki); - - const float & dx = tXj(0,kj); - const float & dy = tXj(1,kj); - const float & dz = tXj(2,kj); - - const float & nx = tXnj(0,kj); - const float & ny = tXnj(1,kj); - const float & nz = tXnj(2,kj); - - - double a = nz*sy - ny*sz; - double b = nx*sz - nz*sx; - double c = ny*sx - nx*sy; - - - double d = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz; - - double angle = nx*snx + ny*sny + nz*snz; - if(angle < 0){continue;} - - double prob = func->getProb(d); - double weight = prob*rwij; - // 0 1 2 3 4 5 - // 6 7 8 9 10 11 - // 12 13 14 15 16 17 - // 18 19 20 21 22 23 - // 24 25 26 27 28 29 - // 30 31 32 33 34 35 - - ATA.coeffRef (0) += weight * a * a; - ATA.coeffRef (1) += weight * a * b; - ATA.coeffRef (2) += weight * a * c; - ATA.coeffRef (3) += weight * a * nx; - ATA.coeffRef (4) += weight * a * ny; - ATA.coeffRef (5) += weight * a * nz; - ATA.coeffRef (7) += weight * b * b; - ATA.coeffRef (8) += weight * b * c; - ATA.coeffRef (9) += weight * b * nx; - ATA.coeffRef (10) += weight * b * ny; - ATA.coeffRef (11) += weight * b * nz; - ATA.coeffRef (14) += weight * c * c; - ATA.coeffRef (15) += weight * c * nx; - ATA.coeffRef (16) += weight * c * ny; - ATA.coeffRef (17) += weight * c * nz; - ATA.coeffRef (21) += weight * nx * nx; - ATA.coeffRef (22) += weight * nx * ny; - ATA.coeffRef (23) += weight * nx * nz; - ATA.coeffRef (28) += weight * ny * ny; - ATA.coeffRef (29) += weight * ny * nz; - ATA.coeffRef (35) += weight * nz * nz; - - ATb.coeffRef (0) += weight * a * d; - ATb.coeffRef (1) += weight * b * d; - ATb.coeffRef (2) += weight * c * d; - ATb.coeffRef (3) += weight * nx * d; - ATb.coeffRef (4) += weight * ny * d; - ATb.coeffRef (5) += weight * nz * d; - - } - ATA.coeffRef (6) = ATA.coeff (1); - ATA.coeffRef (12) = ATA.coeff (2); - ATA.coeffRef (13) = ATA.coeff (8); - ATA.coeffRef (18) = ATA.coeff (3); - ATA.coeffRef (19) = ATA.coeff (9); - ATA.coeffRef (20) = ATA.coeff (15); - ATA.coeffRef (24) = ATA.coeff (4); - ATA.coeffRef (25) = ATA.coeff (10); - ATA.coeffRef (26) = ATA.coeff (16); - ATA.coeffRef (27) = ATA.coeff (22); - ATA.coeffRef (30) = ATA.coeff (5); - ATA.coeffRef (31) = ATA.coeff (11); - ATA.coeffRef (32) = ATA.coeff (17); - ATA.coeffRef (33) = ATA.coeff (23); - ATA.coeffRef (34) = ATA.coeff (29); - } - } - setup_equation_time2 += getTime()-setup_equation_time_start2; - - printf("total_score: %f\n",total_score); - - if(false){ - Eigen::Matrix4d p0inv = poses[0].inverse(); - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - poses[j] = p0inv*poses[j]; - - Eigen::Matrix & tXi = transformed_points[j]; - Eigen::Matrix & Xi = points[j]; - Eigen::Matrix & tXni = transformed_normals[j]; - Eigen::Matrix & Xni = normals[j]; - - Eigen::Matrix4d p = poses[j]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - for(int c = 0; c < Xi.cols(); c++){ - float x = Xi(0,c); - float y = Xi(1,c); - float z = Xi(2,c); - - float nx = Xni(0,c); - float ny = Xni(1,c); - float nz = Xni(2,c); - - tXi(0,c) = m00*x + m01*y + m02*z + m03; - tXi(1,c) = m10*x + m11*y + m12*z + m13; - tXi(2,c) = m20*x + m21*y + m22*z + m23; - - tXni(0,c) = m00*nx + m01*ny + m02*nz; - tXni(1,c) = m10*nx + m11*ny + m12*nz; - tXni(2,c) = m20*nx + m21*ny + m22*nz; - } - } - - std::vector Xv; - for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} - sprintf(buf,"image%5.5i.png",imgcount++); - show(Xv,false,std::string(buf),imgcount); - } - - double solve_equation_time_start = getTime(); - Eigen::MatrixXd fullA = Eigen::MatrixXd::Identity (6 * (nr_frames - 1), 6 * (nr_frames - 1)); - fullA *= 0.00001; - Eigen::VectorXd fullB = Eigen::VectorXd::Zero (6 * (nr_frames - 1)); - - for(unsigned int i = 0; i < nr_frames; i++){ - for(unsigned int j = 0; j == 0 && j < nr_frames; j++){ - if(i == j){continue;} - Matrix6d & ATA = A[i][j]; - Vector6d & ATb = b[i][j]; - Vector6d x = ATA.inverse () * ATb; - //printf("%i %i x: %5.5f %5.5f %5.5f %5.5f %5.5f %5.5f\n",i,j,x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0)); - // Eigen::Matrix4d m = constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0)); - // std::cout << m << std::endl << std::endl; - // printf("%i %i ->%i %i\n",i,j,6*(i-1), 6*j); - if (i > 0){ - //fullA.block (6*(i-1), 6*j,6,6) -= ATA; - fullA.block (6*(i-1),6*(i-1),6,6) += ATA; - fullB.segment (6*(i-1), 6) += ATb; - } - //G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_; - //B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_; - } - } - // std::cout << fullA << std::endl << std::endl; - // std::cout << fullB << std::endl << std::endl; - //printf("--------------------------------------------------\n"); - Eigen::VectorXd fullX = fullA.inverse()*fullB; - for(unsigned int i = 0; i < nr_frames; i++){ - Eigen::Matrix4d m; - if(i == 0){ - m = Eigen::Matrix4d::Identity(); - }else{ - m = constructTransformationMatrix(fullX(6*(i-1)+0),fullX(6*(i-1)+1),fullX(6*(i-1)+2),fullX(6*(i-1)+3),fullX(6*(i-1)+4),fullX(6*(i-1)+5)); - } - //std::cout << m << std::endl << std::endl; - } - - // // Start at 1 because 0 is the reference pose - // for (int vi = 1; vi != n; ++vi) - // { - // for (int vj = 0; vj != n; ++vj) - // { - // // Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge - // Edge e; - // bool present1, present2; - // boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_); - // if (!present1) - // { - // boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_); - // if (!present2) - // continue; - // } - - // // Fill in elements of G and B - // if (vj > 0) - // G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_; - // G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_; - // B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_; - // } - // } - - // // Computation of the linear equation system: GX = B - // // TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse) - // Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B); - - // // Update the poses - // float sum = 0.0; - // for (int vi = 1; vi != n; ++vi) - // { - // Eigen::Vector6f difference_pose = static_cast (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6)); - // sum += difference_pose.norm (); - // setPose (vi, getPose (vi) + difference_pose); - // } - - // for(unsigned int i = 0; i < nr_frames; i++){ - // for(unsigned int j = 0; j < nr_frames; j++){ - // if(i == j){continue;} - // Matrix6d & ATA = A[i][j]; - // Vector6d & ATb = b[i][j]; - // Vector6d x = ATA.inverse () * ATb; - // printf("%i %i x: %5.5f %5.5f %5.5f %5.5f %5.5f %5.5f\n",i,j,x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0)); - - // Matrix6d & ATA2 = A2[i][j]; - // Vector6d & ATb2 = b2[i][j]; - // Vector6d x2 = ATA2.inverse () * ATb2; - // printf("%i %i x: %5.5f %5.5f %5.5f %5.5f %5.5f %5.5f\n",i,j,x2(0,0),x2(1,0),x2(2,0),x2(3,0),x2(4,0),x2(5,0)); - - // Eigen::Matrix4d m = constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0)); - // std::cout << m << std::endl << std::endl; - // } - // } - - solve_equation_time += getTime()-solve_equation_time_start; - - if(false){ - Eigen::Matrix4d p0inv = poses[0].inverse(); - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - poses[j] = p0inv*poses[j]; - - Eigen::Matrix & tXi = transformed_points[j]; - Eigen::Matrix & Xi = points[j]; - Eigen::Matrix & tXni = transformed_normals[j]; - Eigen::Matrix & Xni = normals[j]; - - Eigen::Matrix4d p = poses[j]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - for(int c = 0; c < Xi.cols(); c++){ - float x = Xi(0,c); - float y = Xi(1,c); - float z = Xi(2,c); - - float nx = Xni(0,c); - float ny = Xni(1,c); - float nz = Xni(2,c); - - tXi(0,c) = m00*x + m01*y + m02*z + m03; - tXi(1,c) = m10*x + m11*y + m12*z + m13; - tXi(2,c) = m20*x + m21*y + m22*z + m23; - - tXni(0,c) = m00*nx + m01*ny + m02*nz; - tXni(1,c) = m10*nx + m11*ny + m12*nz; - tXni(2,c) = m20*nx + m21*ny + m22*nz; - } - } - - std::vector Xv; - for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} - sprintf(buf,"image%5.5i.png",imgcount++); - show(Xv,false,std::string(buf),imgcount); - } - //Recover poses from solution - //Recompute points - - //if(isconverged(poses, localposes, stopval, stopval)){break;} - } - - printf("total_time: %5.5f\n",getTime()-total_time_start); - printf("rematch_time: %5.5f\n",rematch_time); - printf("computeModel: %5.5f\n",computeModel_time); - printf("setup_matches_time: %5.5f\n",setup_matches_time); - printf("setup_equation_time: %5.5f\n",setup_equation_time); - printf("setup_equation_time2: %5.5f\n",setup_equation_time2); - printf("solve_equation_time: %5.5f\n",solve_equation_time); - exit(0); - }else{ - for(int outer=0; outer < 30; ++outer) { - if(getTime()-total_time_start > timeout){break;} - - printf("funcupdate: %i rematching: %i lala: %i outer: %i\n",funcupdate,rematching,lala,outer); - std::vector poses2 = poses; - for(unsigned int i = 0; i < nr_frames; i++){ - if(getTime()-total_time_start > timeout){break;} - if(!is_ok[i]){continue;} - unsigned int nr_match = 0; - { - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - std::vector & matchidj = matchids[j][i]; - unsigned int matchesj = matchidj.size(); - std::vector & matchidi = matchids[i][j]; - unsigned int matchesi = matchidi.size(); - - for(unsigned int ki = 0; ki < matchesi; ki++){ - int kj = matchidi[ki]; - if( kj == -1 ){continue;} - if( kj >= matchesj){continue;} - if(!onetoone || matchidj[kj] == ki){ nr_match++;} - } - } - } - - Eigen::Matrix3Xd Xp = Eigen::Matrix3Xd::Zero(3, nr_match); - Eigen::Matrix3Xd Xp_ori = Eigen::Matrix3Xd::Zero(3, nr_match); - Eigen::Matrix3Xd Xn = Eigen::Matrix3Xd::Zero(3, nr_match); - - Eigen::Matrix3Xd Qp = Eigen::Matrix3Xd::Zero(3, nr_match); - Eigen::Matrix3Xd Qn = Eigen::Matrix3Xd::Zero(3, nr_match); - Eigen::VectorXd rangeW = Eigen::VectorXd::Zero( nr_match); - - int count = 0; - - Eigen::Matrix & tXi = transformed_points[i]; - Eigen::Matrix & Xi = points[i]; - Eigen::Matrix & tXni = transformed_normals[i]; - Eigen::Matrix & Xni = normals[i]; - Eigen::VectorXd & informationi = informations[i]; - - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - Eigen::Matrix & tXj = transformed_points[j]; - Eigen::Matrix & tXnj = transformed_normals[j]; - Eigen::VectorXd & informationj = informations[j]; - - std::vector & matchidj = matchids[j][i]; - unsigned int matchesj = matchidj.size(); - std::vector & matchidi = matchids[i][j]; - unsigned int matchesi = matchidi.size(); - - for(unsigned int ki = 0; ki < matchesi; ki++){ - int kj = matchidi[ki]; - if( kj == -1 ){continue;} - if( kj >= matchesj){continue;} - if(!onetoone || matchidj[kj] == ki){ - Qp.col(count) = tXj.col(kj); - Qn.col(count) = tXnj.col(kj); - - Xp_ori.col(count) = Xi.col(ki); - Xp.col(count) = tXi.col(ki); - - Xn.col(count) = tXni.col(ki); - rangeW(count) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj)); - count++; - } - } - } - - if(count == 0){break;} - - //showMatches(Xp,Qp); - for(int inner=0; inner < 5; ++inner) { - Eigen::MatrixXd residuals; - switch(type) { - case PointToPoint: {residuals = Xp-Qp;} break; - case PointToPlane: { - residuals = Eigen::MatrixXd::Zero(1, Xp.cols()); - for(int i=0; igetProbs(residuals); } break; - case PointToPlane: { - W = func->getProbs(residuals); - for(int k=0; k 0.0);} - } break; - default: {printf("type not set\n");} break; - } - - - for(int k=0; k 0.1){good_opt++;} - else{bad_opt++;} - } - - W = W.array()*rangeW.array()*rangeW.array(); - Xo1 = Xp; - switch(type) { - case PointToPoint: { - //RigidMotionEstimator::point_to_point(Xp, Qp, W); - pcl::TransformationFromCorrespondences tfc1; - for(unsigned int c = 0; c < nr_match; c++){tfc1.add(Eigen::Vector3f(Xp(0,c), Xp(1,c),Xp(2,c)),Eigen::Vector3f(Qp(0,c),Qp(1,c),Qp(2,c)),W(c));} - Eigen::Affine3d rot = tfc1.getTransformation().cast(); - Xp = rot*Xp; - Xn = rot.rotation()*Xn; - } break; - case PointToPlane: { - point_to_plane2(Xp, Xn, Qp, Qn, W); - } break; - default: {printf("type not set\n"); } break; - } - - double stop1 = (Xp-Xo1).colwise().norm().maxCoeff(); - Xo1 = Xp; - if(stop1 < 0.001){break; } - } - //exit(0); - pcl::TransformationFromCorrespondences tfc; - for(unsigned int c = 0; c < nr_match; c++){tfc.add(Eigen::Vector3f(Xp_ori(0,c),Xp_ori(1,c),Xp_ori(2,c)),Eigen::Vector3f(Xp(0,c),Xp(1,c),Xp(2,c)));} - poses[i] = tfc.getTransformation().cast().matrix(); - } - - Eigen::Matrix4d p0inv = poses[0].inverse(); - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - poses[j] = p0inv*poses[j]; - - Eigen::Matrix & tXi = transformed_points[j]; - Eigen::Matrix & Xi = points[j]; - Eigen::Matrix & tXni = transformed_normals[j]; - Eigen::Matrix & Xni = normals[j]; - - Eigen::Matrix4d p = poses[j]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - for(int c = 0; c < Xi.cols(); c++){ - float x = Xi(0,c); - float y = Xi(1,c); - float z = Xi(2,c); - - float nx = Xni(0,c); - float ny = Xni(1,c); - float nz = Xni(2,c); - - tXi(0,c) = m00*x + m01*y + m02*z + m03; - tXi(1,c) = m10*x + m11*y + m12*z + m13; - tXi(2,c) = m20*x + m21*y + m22*z + m23; - - tXni(0,c) = m00*nx + m01*ny + m02*nz; - tXni(1,c) = m10*nx + m11*ny + m12*nz; - tXni(2,c) = m20*nx + m21*ny + m22*nz; - } - } - if(isconverged(poses, poses2, stopval, stopval)){break;} - } - } if(isconverged(poses, poses2b, stopval, stopval)){break;} } if(isconverged(poses, poses1, stopval, stopval)){break;} @@ -1376,11 +1754,12 @@ MassFusionResults MassRegistrationPPR::getTransforms(std::vector 0){ std::vector Xv; @@ -1393,6 +1772,7 @@ MassFusionResults MassRegistrationPPR::getTransforms(std::vector guess)\n"); + //exit(0); return MassFusionResults(poses,-1); } diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp new file mode 100644 index 00000000..18b1a4a9 --- /dev/null +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -0,0 +1,2595 @@ +#include "registration/MassRegistrationPPR2.h" + +#include +#include + +#include +#include + +namespace reglib +{ + +MassRegistrationPPR2::MassRegistrationPPR2(double startreg, bool visualize){ + type = PointToPlane; + //type = PointToPoint; + use_PPR_weight = true; + use_features = true; + normalize_matchweights = true; + + + use_surface = true; + use_depthedge = true; + + DistanceWeightFunction2PPR2 * dwf = new DistanceWeightFunction2PPR2(); + dwf->update_size = true; + dwf->startreg = startreg; + dwf->debugg_print = false; + func = dwf; + + + DistanceWeightFunction2PPR2 * kpdwf = new DistanceWeightFunction2PPR2(); + kpdwf->update_size = true; + kpdwf->startreg = startreg; + kpdwf->debugg_print = false; + kpfunc = kpdwf; + + DistanceWeightFunction2PPR2 * depthedge_dwf = new DistanceWeightFunction2PPR2(); + depthedge_dwf->update_size = true; + depthedge_dwf->startreg = startreg; + depthedge_dwf->debugg_print = false; + depthdege_func = depthedge_dwf; + + fast_opt = true; + + nomask = true; + maskstep = 1; + nomaskstep = 100000; + + stopval = 0.001; + steps = 4; + + timeout = 6000; + + if(visualize){ + visualizationLvl = 1; + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0, 0, 0); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + }else{ + visualizationLvl = 0; + } + + + Qp_arr = new double[3*maxcount+0]; + Qn_arr = new double[3*maxcount+0]; + Xp_arr = new double[3*maxcount+0]; + Xn_arr = new double[3*maxcount+0]; + rangeW_arr = new double[maxcount+0]; + + + kp_Qp_arr = new double[3*maxcount+0]; + kp_Qn_arr = new double[3*maxcount+0]; + kp_Xp_arr = new double[3*maxcount+0]; + kp_Xn_arr = new double[3*maxcount+0]; + kp_rangeW_arr = new double[maxcount+0]; + + depthedge_Qp_arr = new double[3*maxcount+0]; + depthedge_Xp_arr = new double[3*maxcount+0]; + depthedge_rangeW_arr = new double[maxcount+0]; +} +MassRegistrationPPR2::~MassRegistrationPPR2(){ + + for(unsigned int i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} + for(unsigned int i = 0; i < arraynormals.size(); i++){delete[] arraynormals[i];} + for(unsigned int i = 0; i < arraycolors.size(); i++){delete[] arraycolors[i];} + for(unsigned int i = 0; i < arrayinformations.size(); i++){delete[] arrayinformations[i];} + + for(unsigned int i = 0; i < trees3d.size(); i++){delete trees3d[i];} + for(unsigned int i = 0; i < a3dv.size(); i++){delete a3dv[i];} + + delete func; + delete kpfunc; + + delete[] Qp_arr; + delete[] Qn_arr; + delete[] Xp_arr; + delete[] Xn_arr; + delete[] rangeW_arr; + + delete[] kp_Qp_arr; + delete[] kp_Qn_arr; + delete[] kp_Xp_arr; + delete[] kp_Xn_arr; + delete[] kp_rangeW_arr; +} + +void MassRegistrationPPR2::addModelData(Model * model_, bool submodels){ + model = model_; + printf("addModelData\n"); + + if(submodels){ + for(unsigned int i = 0; i < model->submodels.size(); i++){ + addData(model->submodels[i]->getPCLnormalcloud(1,false)); + } + }else{ + //setData(model->frames,model->modelmasks); + + for(unsigned int i = 0; i < model->submodels.size(); i++){ + addData(model->submodels[i]->getPCLnormalcloud(1,false)); +// kp_nr_arraypoints.push_back(0); +// kp_arraypoints.push_back(0); +// kp_arraynormals.push_back(0); +// kp_arrayinformations.push_back(0); +// kp_arraydescriptors.push_back(0); +// frameid.push_back(-1); + } + + unsigned int nr_frames = model->frames.size(); + + for(unsigned int i = 0; i < nr_frames; i++){ +// frameid.push_back(i); + addData(model->frames[i], model->modelmasks[i]); + +// RGBDFrame* frame = model->frames[i]; +// std::vector & keypoints = model->all_keypoints[i]; +// cv::Mat & descriptors = model->all_descriptors[i]; +// //uint64_t * descriptorsdata = (uint64_t *)(descriptors.data); + + +// float * normalsdata = (float *)(frame->normals.data); +// unsigned short * depthdata = (unsigned short *)(frame->depth.data); + +// Camera * camera = frame->camera; +// const unsigned int width = camera->width; +// const unsigned int height = camera->height; +// const float idepth = camera->idepth_scale; +// const float cx = camera->cx; +// const float cy = camera->cy; +// const float ifx = 1.0/camera->fx; +// const float ify = 1.0/camera->fy; + +// unsigned int nr_kp = keypoints.size(); +// if(nr_kp){ +// double * ap = new double[3*nr_kp]; +// double * an = new double[3*nr_kp]; +// double * ai = new double[nr_kp]; +// for(unsigned int k = 0; k < nr_kp; k++){ +// cv::KeyPoint & kp = keypoints[k]; +// double w = kp.pt.x; +// double h = kp.pt.y; +// int ind = int(h+0.5)*width+int(w+0.5); + +// float z = idepth*float(depthdata[ind]); +// float x = (w - cx) * z * ifx; +// float y = (h - cy) * z * ify; + +// ap[3*k+0] =x; +// ap[3*k+1] =y; +// ap[3*k+2] =z; + +// an[3*k+0] = normalsdata[3*ind+0]; +// an[3*k+1] = normalsdata[3*ind+1]; +// an[3*k+2] = normalsdata[3*ind+2]; + +// ai[k] = pow(fabs(z),-2); +// } + +// kp_nr_arraypoints.push_back(nr_kp); +// kp_arraypoints.push_back(ap); +// kp_arraynormals.push_back(an); +// kp_arrayinformations.push_back(ai); +// kp_arraydescriptors.push_back((uint64_t *)(descriptors.data)); +// }else{ +// kp_nr_arraypoints.push_back(0); +// kp_arraypoints.push_back(0); +// kp_arraynormals.push_back(0); +// kp_arrayinformations.push_back(0); +// kp_arraydescriptors.push_back(0); +// } + } + +// for(int i = 0; i < is_ok.size(); i++){ +// if(is_ok[i]){printf("%i is ok\n",i); +// }else{printf("%i is not ok\n",i);} +// } + + + } +} + + +void MassRegistrationPPR2::setData(std::vector< pcl::PointCloud::Ptr > all_clouds){ + double total_load_time_start = getTime(); + unsigned int nr_frames = all_clouds.size(); + + if(arraypoints.size() > 0){ + for(unsigned int i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} + arraypoints.resize(0); + } + + if(a3dv.size() > 0){ + for(unsigned int i = 0; i < a3dv.size(); i++){delete a3dv[i];} + a3dv.resize(0); + } + + if(trees3d.size() > 0){ + for(unsigned int i = 0; i < trees3d.size(); i++){delete trees3d[i];} + trees3d.resize(0); + } + + nr_matches.resize(nr_frames); + matchids.resize(nr_frames); + matchdists.resize(nr_frames); + nr_datas.resize(nr_frames); + points.resize(nr_frames); + colors.resize(nr_frames); + normals.resize(nr_frames); + transformed_points.resize(nr_frames); + transformed_normals.resize(nr_frames); + informations.resize(nr_frames); + + nr_arraypoints.resize(nr_frames); + + arraypoints.resize(nr_frames); + arraynormals.resize(nr_frames); + arraycolors.resize(nr_frames); + arrayinformations.resize(nr_frames); + + trees3d.resize(nr_frames); + a3dv.resize(nr_frames); + is_ok.resize(nr_frames); + + for(unsigned int i = 0; i < nr_frames; i++){ + //printf("loading data for %i\n",i); + pcl::PointCloud::Ptr cloud = all_clouds[i]; + int count = 0; + for(unsigned int i = 0; i < cloud->points.size(); i++){ + if (isValidPoint(cloud->points[i])){count++;} + } + + if(count < 10){ + is_ok[i] = false; + continue; + }else{ + is_ok[i] = true; + } + + double * ap = new double[3*count]; + double * an = new double[3*count]; + double * ac = new double[3*count]; + double * ai = new double[3*count]; + + nr_datas[i] = count; + matchids[i].resize(nr_frames); + matchdists[i].resize(nr_frames); + points[i].resize(Eigen::NoChange,count); + colors[i].resize(Eigen::NoChange,count); + normals[i].resize(Eigen::NoChange,count); + transformed_points[i].resize(Eigen::NoChange,count); + transformed_normals[i].resize(Eigen::NoChange,count); + + nr_arraypoints[i] = count; + + arraypoints[i] = ap; + arraynormals[i] = an; + arraycolors[i] = ac; + arrayinformations[i] = ai; + + Eigen::Matrix & X = points[i]; + Eigen::Matrix & C = colors[i]; + Eigen::Matrix & Xn = normals[i]; + + Eigen::VectorXd information (count); + + int c = 0; + for(unsigned int i = 0; i < cloud->points.size(); i++){ + pcl::PointXYZRGBNormal p = cloud->points[i]; + if (isValidPoint(p)){ + + float xn = p.normal_x; + float yn = p.normal_y; + float zn = p.normal_z; + + float x = p.x; + float y = p.y; + float z = p.z; + + ap[3*c+0] =x; + ap[3*c+1] =y; + ap[3*c+2] =z; + + an[3*c+0] =xn; + an[3*c+1] =yn; + an[3*c+2] =zn; + + ac[3*c+0] =p.r; + ac[3*c+1] =p.g; + ac[3*c+2] =p.b; + + ai[c] = 1.0/(x*x+y*y+z*z); + + X(0,c) = x; + X(1,c) = y; + X(2,c) = z; + Xn(0,c) = xn; + Xn(1,c) = yn; + Xn(2,c) = zn; + + information(c) = ai[c];//1.0/(z*z); + C(0,c) = p.r; + C(1,c) = p.g; + C(2,c) = p.b; + c++; + + } + } + + informations[i] = information; + + ArrayData3D * a3d = new ArrayData3D; + a3d->data = ap; + a3d->rows = count; + a3dv[i] = a3d; + trees3d[i] = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + trees3d[i]->buildIndex(); + } + + //printf("total load time: %5.5f\n",getTime()-total_load_time_start); +} + +void MassRegistrationPPR2::setData(std::vector frames_,std::vector mmasks_){ + double total_load_time_start = getTime(); + + unsigned int nr_frames = frames_.size(); + if(arraypoints.size() > 0){ + for(unsigned int i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} + arraypoints.resize(0); + } + + if(a3dv.size() > 0){ + for(unsigned int i = 0; i < a3dv.size(); i++){delete a3dv[i];} + a3dv.resize(0); + } + + if(trees3d.size() > 0){ + for(unsigned int i = 0; i < trees3d.size(); i++){delete trees3d[i];} + trees3d.resize(0); + } + + for(unsigned int i = 0; i < nr_frames; i++){addData(frames_[i], mmasks_[i]);} + //printf("total load time: %5.5f\n",getTime()-total_load_time_start); +} + +void matchframes(Eigen::Affine3d rp, int nr_ap, double * ap, std::vector & matchid, std::vector & matchdist, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + matchid.resize(nr_ap); + matchdist.resize(nr_ap); + +#pragma omp parallel for num_threads(8) + for(unsigned int k = 0; k < nr_ap; ++k) { + int prev = matchid[k]; + double qp [3]; + const double & src_x = ap[3*k+0]; + const double & src_y = ap[3*k+1]; + const double & src_z = ap[3*k+2]; + + qp[0] = m00*src_x + m01*src_y + m02*src_z + m03; + qp[1] = m10*src_x + m11*src_y + m12*src_z + m13; + qp[2] = m20*src_x + m21*src_y + m22*src_z + m23; + + size_t ret_index; double out_dist_sqr; + nanoflann::KNNResultSet resultSet(1); + resultSet.init(&ret_index, &out_dist_sqr ); + t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + + int current = ret_index; + new_good_rematches += prev != current; + new_total_rematches++; + matchid[k] = current; + matchdist[k] = out_dist_sqr; + } +} + +void MassRegistrationPPR2::rematch(std::vector poses, std::vector prev_poses, bool first){ + //printf("rematch\n"); + double new_good_rematches = 0; + double new_total_rematches = 0; + unsigned int nr_frames = poses.size(); + + int rmt = 2; + + if(rmt==2){ + int ignores = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + nr_matches[i] = 0; + + if(use_depthedge){ + + } + if(use_surface){ + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + if(!first){ + Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); + + Eigen::Affine3d diff = prev_rp.inverse()*rp; + + double change_trans = 0; + double change_rot = 0; + double dt = 0; + for(unsigned int k = 0; k < 3; k++){ + dt += diff(k,3)*diff(k,3); + for(unsigned int l = 0; l < 3; l++){ + if(k == l){ change_rot += fabs(1-diff(k,l));} + else{ change_rot += fabs(diff(k,l));} + } + } + change_trans += sqrt(dt); + + //if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;} + } + + //prev_poses[j] = poses[j]; + matchframes(rp, nr_arraypoints[i], arraypoints[i], matchids[i][j], matchdists[i][j],trees3d[j], new_good_rematches,new_total_rematches); +/* + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + const unsigned int nr_ap = nr_arraypoints[i]; + double * ap = arraypoints[i]; + + std::vector & matchid = matchids[i][j]; + std::vector & matchdist = matchdists[i][j]; + matchid.resize(nr_ap); + matchdist.resize(nr_ap); + Tree3d * t3d = trees3d[j]; + +#pragma omp parallel for num_threads(8) + for(unsigned int k = 0; k < nr_ap; ++k) { + int prev = matchid[k]; + double qp [3]; + const double & src_x = ap[3*k+0]; + const double & src_y = ap[3*k+1]; + const double & src_z = ap[3*k+2]; + + qp[0] = m00*src_x + m01*src_y + m02*src_z + m03; + qp[1] = m10*src_x + m11*src_y + m12*src_z + m13; + qp[2] = m20*src_x + m21*src_y + m22*src_z + m23; + + size_t ret_index; double out_dist_sqr; + nanoflann::KNNResultSet resultSet(1); + resultSet.init(&ret_index, &out_dist_sqr ); + t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + + int current = ret_index; + new_good_rematches += prev != current; + new_total_rematches++; + matchid[k] = current; + matchdist[k] = out_dist_sqr; + } + nr_matches[i] += matchid.size(); + */ + } + } + } + //printf("ignores: %i rematches: %i ratio of good rematches: %4.4f\n",ignores,nr_frames*(nr_frames-1)-ignores,new_good_rematches/new_total_rematches); + } + + if(rmt==1){ + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + nr_matches[i] = 0; + + double * ap = arraypoints[i]; + const unsigned int nr_ap = nr_arraypoints[i]; + + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + std::vector & matchid = matchids[i][j]; + matchid.resize(nr_ap); + Tree3d * t3d = trees3d[j]; + + for(unsigned int k = 0; k < nr_ap; ++k) { + int prev = matchid[k]; + double qp [3]; + const double & src_x = ap[3*k+0]; + const double & src_y = ap[3*k+1]; + const double & src_z = ap[3*k+2]; + + qp[0] = m00*src_x + m01*src_y + m02*src_z + m03; + qp[1] = m10*src_x + m11*src_y + m12*src_z + m13; + qp[2] = m20*src_x + m21*src_y + m22*src_z + m23; + + size_t ret_index; double out_dist_sqr; + nanoflann::KNNResultSet resultSet(1); + resultSet.init(&ret_index, &out_dist_sqr ); + t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + + int current = ret_index; + new_good_rematches += prev != current; + new_total_rematches++; + matchid[k] = current; + } + nr_matches[i] += matchid.size(); + } + } + } + if(rmt==0){ + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + nr_matches[i] = 0; + + double * ap = arraypoints[i]; + int nr_ap = nr_arraypoints[i]; + + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + Eigen::Matrix tX = rp*points[i]; + + unsigned int nr_data = nr_datas[i]; + std::vector & matchid = matchids[i][j]; + matchid.resize(nr_data); + Tree3d * t3d = trees3d[j]; + + for(unsigned int k = 0; k < nr_data; ++k) { + int prev = matchid[k]; + double * qp = tX.col(k).data(); + + size_t ret_index; double out_dist_sqr; + nanoflann::KNNResultSet resultSet(1); + resultSet.init(&ret_index, &out_dist_sqr ); + t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + + int current = ret_index; + new_good_rematches += prev != current; + new_total_rematches++; + matchid[k] = current; + } + nr_matches[i] += matchid.size(); + } + } + } + // good_rematches += new_good_rematches; + // total_rematches += new_total_rematches +} + + +Eigen::MatrixXd MassRegistrationPPR2::getAllResiduals(std::vector poses){ + unsigned int nr_frames = poses.size(); + Eigen::MatrixXd all_residuals; + + int total_matches = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + total_matches += matchids[i][j].size(); + } + } + + int all_residuals_type = 1; + + if(all_residuals_type == 1){ + switch(type) { + case PointToPoint: {all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);}break; + case PointToPlane: {all_residuals = Eigen::MatrixXd::Zero(1,total_matches);}break; + default: {printf("type not set\n");} break; + } + + int count = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + + double * api = arraypoints[i]; + double * ani = arraynormals[i]; + double * aci = arraycolors[i]; + double * aii = arrayinformations[i]; + const unsigned int nr_api = nr_arraypoints[i]; + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + + double * apj = arraypoints[j]; + double * anj = arraynormals[j]; + double * acj = arraycolors[j]; + double * aij = arrayinformations[j]; + const unsigned int nr_apj = nr_arraypoints[j]; + + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + if(type == PointToPlane){ + for(unsigned int ki = 0; ki < nr_api; ++ki) { + int kj = matchidi[ki]; + if( kj < 0 || kj >= nr_apj){continue;} + const double & src_x = api[3*ki+0]; + const double & src_y = api[3*ki+1]; + const double & src_z = api[3*ki+2]; + + const double & src_nx = ani[3*ki+0]; + const double & src_ny = ani[3*ki+1]; + const double & src_nz = ani[3*ki+2]; + + const double & info_i = aii[ki]; + + const double & dst_x = apj[3*kj+0]; + const double & dst_y = apj[3*kj+1]; + const double & dst_z = apj[3*kj+2]; + + + const double & info_j = aij[kj]; + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + float txn = m00*src_nx + m01*src_ny + m02*src_nz; + float tyn = m10*src_nx + m11*src_ny + m12*src_nz; + float tzn = m20*src_nx + m21*src_ny + m22*src_nz; + + const double rw = 1.0/(1.0/info_i+1.0/info_j); + const double di = (txn*(tx-dst_x) + tyn*(ty-dst_y) + tzn*(tz-dst_z))*rw; + all_residuals(0,count) = di; + count++; + } + } + + if(type == PointToPoint){ + for(unsigned int ki = 0; ki < nr_api; ++ki) { + int kj = matchidi[ki]; + if( kj < 0 ){continue;} + const double & src_x = api[3*ki+0]; + const double & src_y = api[3*ki+1]; + const double & src_z = api[3*ki+2]; + + const double & info_i = aii[ki]; + + const double & dst_x = apj[3*kj+0]; + const double & dst_y = apj[3*kj+1]; + const double & dst_z = apj[3*kj+2]; + + const double & info_j = aij[kj]; + + float tx = m00*src_x + m01*src_y + m02*src_z + m03;// const unsigned int src_nr_ap = kp_nr_arraypoints[i]; + // if(src_nr_ap == 0){continue;} + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + const double rw = 1.0/(1.0/info_i+1.0/info_j); + all_residuals(0,count) = (tx-dst_x)*rw; + all_residuals(1,count) = (ty-dst_y)*rw; + all_residuals(2,count) = (tz-dst_z)*rw; + count++; + } + } + } + } + } + + if(all_residuals_type == 0){ + switch(type) { + case PointToPoint: {all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);}break; + case PointToPlane: {all_residuals = Eigen::MatrixXd::Zero(1,total_matches);}break; + default: {printf("type not set\n");} break; + } + + int count = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + Eigen::Matrix & tXi = transformed_points[i]; + Eigen::Matrix & tXni = transformed_normals[i]; + Eigen::VectorXd & informationi = informations[i]; + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + Eigen::Matrix & tXj = transformed_points[j]; + Eigen::Matrix & tXnj = transformed_normals[j]; + Eigen::VectorXd & informationj = informations[j]; + Eigen::Matrix3Xd Xp = Eigen::Matrix3Xd::Zero(3, matchesi); + Eigen::Matrix3Xd Xn = Eigen::Matrix3Xd::Zero(3, matchesi); + Eigen::Matrix3Xd Qp = Eigen::Matrix3Xd::Zero(3, matchesi); + Eigen::Matrix3Xd Qn = Eigen::Matrix3Xd::Zero(3, matchesi); + Eigen::VectorXd rangeW = Eigen::VectorXd::Zero( matchesi); + + for(unsigned int ki = 0; ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( ki >= Qp.cols() || kj < 0 || kj >= tXj.cols() ){continue;} + Qp.col(ki) = tXj.col(kj); + Qn.col(ki) = tXnj.col(kj); + Xp.col(ki) = tXi.col(ki); + Xn.col(ki) = tXni.col(ki); + rangeW(ki) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj)); + } + Eigen::MatrixXd residuals; + switch(type) { + case PointToPoint: {residuals = Xp-Qp;} break; + case PointToPlane: { + residuals = Eigen::MatrixXd::Zero(1, Xp.cols()); + for(int i=0; i poses){ + unsigned int nr_frames = poses.size(); + + +// printf("is_ok size: %i\n",is_ok.size()); + int total_matches = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + const unsigned int src_nr_ap = kp_nr_arraypoints[i]; + if(src_nr_ap == 0){continue;} + for(unsigned int j = 0; j < nr_frames; j++){ + const unsigned int dst_nr_ap = kp_nr_arraypoints[j]; + if(dst_nr_ap == 0){continue;} + total_matches += kp_matches[i][j].size(); + } + } + + + +// printf("total matches: %i\n",total_matches); + + +// total_matches = 0; +// for(unsigned int i = 0; i < nr_frames; i++){ +// for(unsigned int j = 0; j < nr_frames; j++){ +// total_matches += kp_matches[i][j].size(); +// } +// } + + +// printf("total matches: %i\n",total_matches); + +// exit(0); + + int count = 0; + Eigen::MatrixXd all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches); +//if(total_matches == 0){return all_residuals;} + + int ignores = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + const unsigned int src_nr_ap = kp_nr_arraypoints[i]; + if(src_nr_ap == 0){continue;} + double * src_ap = kp_arraypoints[i]; + double * src_ai = kp_arrayinformations[i]; + + for(unsigned int j = 0; j < nr_frames; j++){ + if(i == j){continue;} + + const unsigned int dst_nr_ap = kp_nr_arraypoints[j]; + if(dst_nr_ap == 0){continue;} + +// printf("nr kp(%i %i): %i %i\n",i,j,src_nr_ap,dst_nr_ap); + double * dst_ap = kp_arraypoints[j]; + double * dst_ai = kp_arrayinformations[j]; + + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + std::vector< TestMatch > & matches = kp_matches[i][j]; + unsigned int nr_matches = matches.size(); + + for(unsigned int k = 0; k < nr_matches; ++k) { + TestMatch & tm = matches[k]; + unsigned int src_k = tm.src; + unsigned int dst_k = tm.dst; + + const double & src_x = src_ap[3*src_k+0]; + const double & src_y = src_ap[3*src_k+1]; + const double & src_z = src_ap[3*src_k+2]; + const double & src_info = src_ai[src_k]; + + double sx = m00*src_x + m01*src_y + m02*src_z + m03; + double sy = m10*src_x + m11*src_y + m12*src_z + m13; + double sz = m20*src_x + m21*src_y + m22*src_z + m23; + + double dx = sx-dst_ap[3*dst_k+0]; + double dy = sy-dst_ap[3*dst_k+1]; + double dz = sz-dst_ap[3*dst_k+2]; + + const double & dst_info = dst_ai[dst_k]; + const double rw = 1.0/(1.0/src_info+1.0/dst_info); +// if(total_matches <= count){printf("strange\n");} + all_residuals(0,count) = dx*rw; + all_residuals(1,count) = dy*rw; + all_residuals(2,count) = dz*rw; + count++; + } + } + } + return all_residuals; +} + +void MassRegistrationPPR2::clearData(){} + +void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ + + double total_load_time_start = getTime(); + frames.push_back(frame); + mmasks.push_back(mmask); + + nr_matches.push_back( 0); + matchids.push_back( std::vector< std::vector >() ); + matchdists.push_back( std::vector< std::vector >() ); + nr_datas.push_back( 0); + + points.push_back( Eigen::Matrix()); + colors.push_back( Eigen::Matrix()); + normals.push_back( Eigen::Matrix()); + transformed_points.push_back( Eigen::Matrix()); + transformed_normals.push_back( Eigen::Matrix()); + + informations.push_back( Eigen::VectorXd()); + + nr_arraypoints.push_back(0); + arraypoints.push_back(0); + arraynormals.push_back(0); + arraycolors.push_back(0); + arrayinformations.push_back(0); + trees3d.push_back(0); + a3dv.push_back(0); + + depthedge_nr_arraypoints.push_back(0); + depthedge_arraypoints.push_back(0); + depthedge_arrayinformations.push_back(0); + depthedge_trees3d.push_back(0); + depthedge_a3dv.push_back(0); + + + is_ok.push_back(false); + + unsigned int i = points.size()-1; + + bool * maskvec = mmask->maskvec; + unsigned char * edgedata = (unsigned char *)(frame->depthedges.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + int count = 0; + for(unsigned int w = 0; w < width; w+=maskstep){ + for(unsigned int h = 0; h < height; h+=maskstep){ + int ind = h*width+w; + if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ + float z = idepth*float(depthdata[ind]); + float xn = normalsdata[3*ind+0]; + if(z > 0.2 && xn != 2){count++;} + } + } + } + + + if(count < 10){ + is_ok[i] = false; + return; + }else{ + is_ok[i] = true; + } + +// printf("%i is ok %i\n",i,is_ok[i]); + + nr_datas[i] = count; + points[i].resize(Eigen::NoChange,count); + colors[i].resize(Eigen::NoChange,count); + normals[i].resize(Eigen::NoChange,count); + transformed_points[i].resize(Eigen::NoChange,count); + transformed_normals[i].resize(Eigen::NoChange,count); + + double * ap = new double[3*count]; + double * an = new double[3*count]; + double * ac = new double[3*count]; + double * ai = new double[3*count]; + nr_arraypoints[i] = count; + arraypoints[i] = ap; + arraynormals[i] = an; + arraycolors[i] = ac; + arrayinformations[i] = ai; + + Eigen::Matrix & X = points[i]; + Eigen::Matrix & C = colors[i]; + Eigen::Matrix & Xn = normals[i]; + Eigen::Matrix & tX = transformed_points[i]; + Eigen::Matrix & tXn = transformed_normals[i]; + Eigen::VectorXd information (count); + + int c = 0; + for(unsigned int w = 0; w < width; w+=maskstep){ + for(unsigned int h = 0; h < height;h+=maskstep){ + if(c == count){continue;} + int ind = h*width+w; + //if(maskvec[ind] || nomask){ + if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ + float z = idepth*float(depthdata[ind]); + float xn = normalsdata[3*ind+0]; + + if(z > 0.2 && xn != 2){ + float yn = normalsdata[3*ind+1]; + float zn = normalsdata[3*ind+2]; + + float x = (w - cx) * z * ifx; + float y = (h - cy) * z * ify; + + ap[3*c+0] =x; + ap[3*c+1] =y; + ap[3*c+2] =z; + + an[3*c+0] =xn; + an[3*c+1] =yn; + an[3*c+2] =zn; + + ac[3*c+0] =rgbdata[3*ind+2]; + ac[3*c+1] =rgbdata[3*ind+1]; + ac[3*c+2] =rgbdata[3*ind+0]; + + ai[c] = pow(fabs(z),-2);//1.0/(z*z); + + X(0,c) = x; + X(1,c) = y; + X(2,c) = z; + Xn(0,c) = xn; + Xn(1,c) = yn; + Xn(2,c) = zn; + + information(c) = ai[c];//1.0/(z*z); + C(0,c) = rgbdata[3*ind+0]; + C(1,c) = rgbdata[3*ind+1]; + C(2,c) = rgbdata[3*ind+2]; + c++; + } + } + } + } + informations[i] = information; + + ArrayData3D * a3d = new ArrayData3D; + a3d->data = ap; + a3d->rows = count; + a3dv[i] = a3d; + trees3d[i] = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + trees3d[i]->buildIndex(); + + + int depthedge_count = 0; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height; h++){ + int ind = h*width+w; + if(maskvec[ind] && edgedata[ind] == 255){ + float z = idepth*float(depthdata[ind]); + if(z > 0.2){depthedge_count++;} + } + } + } + + if(depthedge_count < 10){ + double * depthedge_ap = new double[3*depthedge_count]; + double * depthedge_ai = new double[3*depthedge_count]; + depthedge_nr_arraypoints[i] = depthedge_count; + depthedge_arraypoints[i] = depthedge_ap; + depthedge_arrayinformations[i] = depthedge_ai; + + c = 0; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height; h++){ + if(c == depthedge_count){continue;} + int ind = h*width+w; + if(maskvec[ind] && edgedata[ind] == 255){ + float z = idepth*float(depthdata[ind]); + if(z > 0.2){ + depthedge_ap[3*c+0] = (w - cx) * z * ifx; + depthedge_ap[3*c+1] = (h - cy) * z * ify;; + depthedge_ap[3*c+2] = z; + depthedge_ai[c] = pow(fabs(z),-2);//1.0/(z*z); + c++; + } + } + } + } + + ArrayData3D * depthedge_a3d = new ArrayData3D; + depthedge_a3d->data = depthedge_ap; + depthedge_a3d->rows = depthedge_count; + depthedge_a3dv[i] = depthedge_a3d; + depthedge_trees3d[i] = new Tree3d(3, *depthedge_a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + depthedge_trees3d[i]->buildIndex(); + } + + //printf("total load time: %5.5f\n",getTime()-total_load_time_start); +} + +void MassRegistrationPPR2::addData(pcl::PointCloud::Ptr cloud){ + double total_load_time_start = getTime(); + + nr_matches.push_back( 0); + matchids.push_back( std::vector< std::vector >() ); + matchdists.push_back( std::vector< std::vector >() ); + nr_datas.push_back( 0); + + points.push_back( Eigen::Matrix()); + colors.push_back( Eigen::Matrix()); + normals.push_back( Eigen::Matrix()); + transformed_points.push_back( Eigen::Matrix()); + transformed_normals.push_back( Eigen::Matrix()); + + informations.push_back( Eigen::VectorXd()); + + nr_arraypoints.push_back(0); + + arraypoints.push_back(0); + arraynormals.push_back(0); + arraycolors.push_back(0); + arrayinformations.push_back(0); + + trees3d.push_back(0); + a3dv.push_back(0); + is_ok.push_back(false); + + unsigned int i = points.size()-1; + + int count = 0; + for(unsigned int j = 0; j < cloud->points.size(); j+=nomaskstep*nomaskstep){ + if (isValidPoint(cloud->points[j])){count++;} + } + + printf("count: %i\n",count); + + if(count < 10){ + is_ok[i] = false; + return; + }else{ + is_ok[i] = true; + } + + + + double * ap = new double[3*count]; + double * an = new double[3*count]; + double * ac = new double[3*count]; + double * ai = new double[3*count]; + + nr_datas[i] = count; + points[i].resize(Eigen::NoChange,count); + colors[i].resize(Eigen::NoChange,count); + normals[i].resize(Eigen::NoChange,count); + transformed_points[i].resize(Eigen::NoChange,count); + transformed_normals[i].resize(Eigen::NoChange,count); + + nr_arraypoints[i] = count; + + arraypoints[i] = ap; + arraynormals[i] = an; + arraycolors[i] = ac; + arrayinformations[i] = ai; + + Eigen::Matrix & X = points[i]; + Eigen::Matrix & C = colors[i]; + Eigen::Matrix & Xn = normals[i]; + + Eigen::VectorXd information (count); + + int c = 0; + for(unsigned int j = 0; j < cloud->points.size(); j+=nomaskstep*nomaskstep){ + pcl::PointXYZRGBNormal p = cloud->points[j]; + if (isValidPoint(p)){ + + float xn = p.normal_x; + float yn = p.normal_y; + float zn = p.normal_z; + + float x = p.x; + float y = p.y; + float z = p.z; + + //if(j % 1000 == 0){printf("%i -> %f %f %f\n",j,x,y,z);} + + ap[3*c+0] =x; + ap[3*c+1] =y; + ap[3*c+2] =z; + + an[3*c+0] =xn; + an[3*c+1] =yn; + an[3*c+2] =zn; + + ac[3*c+0] =p.r; + ac[3*c+1] =p.g; + ac[3*c+2] =p.b; + + ai[c] = 1/sqrt(x*x+y*y+z*z); + + X(0,c) = x; + X(1,c) = y; + X(2,c) = z; + Xn(0,c) = xn; + Xn(1,c) = yn; + Xn(2,c) = zn; + + information(c) = ai[c];//1/sqrt(2+x*x+y*y+z*z);//1.0/(z*z); + C(0,c) = p.r; + C(1,c) = p.g; + C(2,c) = p.b; + c++; + + } + } + + informations[i] = information; + + kp_nr_arraypoints.push_back(0); + kp_arraypoints.push_back(0); + kp_arraynormals.push_back(0); + kp_arrayinformations.push_back(0); + kp_arraydescriptors.push_back(0); + + ArrayData3D * a3d = new ArrayData3D; + a3d->data = ap; + a3d->rows = count; + a3dv[i] = a3d; + trees3d[i] = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + trees3d[i]->buildIndex(); + if(visualizationLvl > 0){ + printf("total load time: %5.5f\n",getTime()-total_load_time_start); + } +} + +std::vector MassRegistrationPPR2::optimize(std::vector poses){ + bool onetoone = true; + unsigned int nr_frames = poses.size(); + Eigen::MatrixXd Xo1; + + int optt = 2; + for(int outer=0; outer < 60; ++outer) { + if(getTime()-total_time_start > timeout){break;} + + //printf("outer: %i\n",outer); + + std::vector poses2 = poses; + if(type == PointToPlane && optt == 2){ + for(unsigned int i = 0; i < nr_frames; i++){ + if(getTime()-total_time_start > timeout){break;} + if(!is_ok[i]){continue;} + + unsigned int count = 0; + double * api = arraypoints[i]; + double * ani = arraynormals[i]; + double * aci = arraycolors[i]; + double * aii = arrayinformations[i]; + const unsigned int nr_api = nr_arraypoints[i]; + + unsigned int kp_count = 0; +// double * kp_api = kp_arraypoints[i]; +// double * kp_ani = kp_arraynormals[i]; +// double * kp_aii = kp_arrayinformations[i]; +// const unsigned int kp_nr_api = kp_nr_arraypoints[i]; + + Eigen::Affine3d rpi = Eigen::Affine3d(poses[i]); + const double & mi00 = rpi(0,0); const double & mi01 = rpi(0,1); const double & mi02 = rpi(0,2); const double & mi03 = rpi(0,3); + const double & mi10 = rpi(1,0); const double & mi11 = rpi(1,1); const double & mi12 = rpi(1,2); const double & mi13 = rpi(1,3); + const double & mi20 = rpi(2,0); const double & mi21 = rpi(2,1); const double & mi22 = rpi(2,2); const double & mi23 = rpi(2,3); + + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + + double * apj = arraypoints[j]; + double * anj = arraynormals[j]; + double * acj = arraycolors[j]; + double * aij = arrayinformations[j]; + const unsigned int nr_apj = nr_arraypoints[j]; + +// double * kp_apj = kp_arraypoints[j]; +// double * kp_anj = kp_arraynormals[j]; +// double * kp_aij = kp_arrayinformations[j]; +// const unsigned int kp_nr_apj = kp_nr_arraypoints[j]; + + Eigen::Affine3d rpj = Eigen::Affine3d(poses[j]); + const double & mj00 = rpj(0,0); const double & mj01 = rpj(0,1); const double & mj02 = rpj(0,2); const double & mj03 = rpj(0,3); + const double & mj10 = rpj(1,0); const double & mj11 = rpj(1,1); const double & mj12 = rpj(1,2); const double & mj13 = rpj(1,3); + const double & mj20 = rpj(2,0); const double & mj21 = rpj(2,1); const double & mj22 = rpj(2,2); const double & mj23 = rpj(2,3); + + std::vector & matchidj = matchids[j][i]; + unsigned int matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + + std::vector & matchdistj = matchdists[j][i]; + std::vector & matchdisti = matchdists[i][j]; + + for(unsigned int ki = 0; true && ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( kj == -1 ){continue;} + if( kj >= matchesj){continue;} + if(!onetoone || matchidj[kj] == ki){ + Xp_arr[3*count+0] = api[3*ki+0]; + Xp_arr[3*count+1] = api[3*ki+1]; + Xp_arr[3*count+2] = api[3*ki+2]; + + Xn_arr[3*count+0] = ani[3*ki+0]; + Xn_arr[3*count+1] = ani[3*ki+1]; + Xn_arr[3*count+2] = ani[3*ki+2]; + + const double & info_i = aii[ki]; + + const double & dst_x = apj[3*kj+0]; + const double & dst_y = apj[3*kj+1]; + const double & dst_z = apj[3*kj+2]; + + const double & dst_nx = anj[3*kj+0]; + const double & dst_ny = anj[3*kj+1]; + const double & dst_nz = anj[3*kj+2]; + + const double & info_j = aij[kj]; + + Qp_arr[3*count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03; + Qp_arr[3*count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13; + Qp_arr[3*count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23; + + Qn_arr[3*count+0] = mj00*dst_nx + mj01*dst_ny + mj02*dst_nz; + Qn_arr[3*count+1] = mj10*dst_nx + mj11*dst_ny + mj12*dst_nz; + Qn_arr[3*count+2] = mj20*dst_nx + mj21*dst_ny + mj22*dst_nz; + + rangeW_arr[count] = 1.0/(1.0/info_i+1.0/info_j); + + count++; + } + } +/* + if(kp_matches.size() > 0 && kp_matches[i].size() > 0 && kp_matches[i][j].size() > 0){ + std::vector< TestMatch > & matches = kp_matches[i][j]; + unsigned int nr_matches = matches.size(); + for(unsigned int k = 0; k < nr_matches; k++){ + TestMatch & m = matches[k]; + int ki = m.src; + int kj = m.dst; + + kp_Xp_arr[3*kp_count+0] = kp_api[3*ki+0]; + kp_Xp_arr[3*kp_count+1] = kp_api[3*ki+1]; + kp_Xp_arr[3*kp_count+2] = kp_api[3*ki+2]; + + kp_Xn_arr[3*kp_count+0] = kp_ani[3*ki+0]; + kp_Xn_arr[3*kp_count+1] = kp_ani[3*ki+1]; + kp_Xn_arr[3*kp_count+2] = kp_ani[3*ki+2]; + const double & info_i = kp_aii[ki]; + + const double & dst_x = kp_apj[3*kj+0]; + const double & dst_y = kp_apj[3*kj+1]; + const double & dst_z = kp_apj[3*kj+2]; + + const double & dst_nx = kp_anj[3*kj+0]; + const double & dst_ny = kp_anj[3*kj+1]; + const double & dst_nz = kp_anj[3*kj+2]; + + const double & info_j = kp_aij[kj]; + + kp_Qp_arr[3*kp_count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03; + kp_Qp_arr[3*kp_count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13; + kp_Qp_arr[3*kp_count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23; + + kp_Qn_arr[3*kp_count+0] = mj00*dst_nx + mj01*dst_ny + mj02*dst_nz; + kp_Qn_arr[3*kp_count+1] = mj10*dst_nx + mj11*dst_ny + mj12*dst_nz; + kp_Qn_arr[3*kp_count+2] = mj20*dst_nx + mj21*dst_ny + mj22*dst_nz; + + kp_rangeW_arr[kp_count] = 1.0/(1.0/info_i+1.0/info_j); + kp_count++; + } + } +*/ + } + if(count == 0 && kp_count == 0){break;} + + typedef Eigen::Matrix Vector6d; + typedef Eigen::Matrix Matrix6d; + + Matrix6d ATA; + Vector6d ATb; + + //std::vector Xv; + //if(visualizationLvl == 4){for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + + + + Eigen::Affine3d p = rpi; + bool do_inner = true; + for(int inner=0; inner < 15 && do_inner; ++inner) { + + pcl::PointCloud::Ptr scloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + + ATA.setZero (); + ATb.setZero (); + const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3); + const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3); + const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3); + double planeNoise = func->getNoise(); + double planeInfo = 1.0/(planeNoise*planeNoise); + + + double kpNoise = kpfunc->getNoise(); + double kpInfo = 1.0/(kpNoise*kpNoise); + + for(unsigned int co = 0; co < count; co++){ + const double & src_x = Xp_arr[3*co+0]; + const double & src_y = Xp_arr[3*co+1]; + const double & src_z = Xp_arr[3*co+2]; + const double & src_nx = Xn_arr[3*co+0]; + const double & src_ny = Xn_arr[3*co+1]; + const double & src_nz = Xn_arr[3*co+2]; + + const double & dx = Qp_arr[3*co+0]; + const double & dy = Qp_arr[3*co+1]; + const double & dz = Qp_arr[3*co+2]; + const double & dnx = Qn_arr[3*co+0]; + const double & dny = Qn_arr[3*co+1]; + const double & dnz = Qn_arr[3*co+2]; + + const double & rw = rangeW_arr[co]; + + const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; + const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; + const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; + + const double & nx = m00*src_nx + m01*src_ny + m02*src_nz; + const double & ny = m10*src_nx + m11*src_ny + m12*src_nz; + const double & nz = m20*src_nx + m21*src_ny + m22*src_nz; + + const double & angle = nx*dnx+ny*dny+nz*dnz; + //printf("%f\n",angle); + //if(angle < 0){exit(0);continue;} + + if(angle < 0){continue;} + + double di = rw*(nx*(sx-dx) + ny*(sy-dy) + nz*(sz-dz)); + //double weight = angle*angle*angle*angle*func->getProb(di)*rw*rw; + double prob = func->getProb(di); + double weight = planeInfo*prob*rw*rw; + + if(visualizationLvl == 5 && i == 0){ + pcl::PointXYZRGBNormal p; + p.x = sx; + p.y = sy; + p.z = sz; + p.b = 0; + p.g = 255; + p.r = 0; + scloud->points.push_back(p); + + pcl::PointXYZRGBNormal p1; + p1.x = dx; + p1.y = dy; + p1.z = dz; + p1.b = 255.0*prob; + p1.g = 255.0*prob; + p1.r = 255.0*prob; + dcloud->points.push_back(p1); + } + + const double & a = nz*sy - ny*sz; + const double & b = nx*sz - nz*sx; + const double & c = ny*sx - nx*sy; + + ATA.coeffRef (0) += weight * a * a; + ATA.coeffRef (1) += weight * a * b; + ATA.coeffRef (2) += weight * a * c; + ATA.coeffRef (3) += weight * a * nx; + ATA.coeffRef (4) += weight * a * ny; + ATA.coeffRef (5) += weight * a * nz; + ATA.coeffRef (7) += weight * b * b; + ATA.coeffRef (8) += weight * b * c; + ATA.coeffRef (9) += weight * b * nx; + ATA.coeffRef (10) += weight * b * ny; + ATA.coeffRef (11) += weight * b * nz; + ATA.coeffRef (14) += weight * c * c; + ATA.coeffRef (15) += weight * c * nx; + ATA.coeffRef (16) += weight * c * ny; + ATA.coeffRef (17) += weight * c * nz; + ATA.coeffRef (21) += weight * nx * nx; + ATA.coeffRef (22) += weight * nx * ny; + ATA.coeffRef (23) += weight * nx * nz; + ATA.coeffRef (28) += weight * ny * ny; + ATA.coeffRef (29) += weight * ny * nz; + ATA.coeffRef (35) += weight * nz * nz; + + const double & d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz); + + ATb.coeffRef (0) += a * d; + ATb.coeffRef (1) += b * d; + ATb.coeffRef (2) += c * d; + ATb.coeffRef (3) += nx * d; + ATb.coeffRef (4) += ny * d; + ATb.coeffRef (5) += nz * d; + } + + double wsum = 0; + double wsx = 0; + double wsy = 0; + double wsz = 0; + for(unsigned int co = 0; co < kp_count; co++){ + const double & src_x = kp_Xp_arr[3*co+0]; + const double & src_y = kp_Xp_arr[3*co+1]; + const double & src_z = kp_Xp_arr[3*co+2]; + + const double & src_nx = kp_Xn_arr[3*co+0]; + const double & src_ny = kp_Xn_arr[3*co+1]; + const double & src_nz = kp_Xn_arr[3*co+2]; + + + const double & dx = kp_Qp_arr[3*co+0]; + const double & dy = kp_Qp_arr[3*co+1]; + const double & dz = kp_Qp_arr[3*co+2]; + + const double & dnx = kp_Qn_arr[3*co+0]; + const double & dny = kp_Qn_arr[3*co+1]; + const double & dnz = kp_Qn_arr[3*co+2]; + + const double & rw = kp_rangeW_arr[co]; + + const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; + const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; + const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; + + const double & nx = m00*src_nx + m01*src_ny + m02*src_nz; + const double & ny = m10*src_nx + m11*src_ny + m12*src_nz; + const double & nz = m20*src_nx + m21*src_ny + m22*src_nz; + + const double & angle = nx*dnx+ny*dny+nz*dnz; + + + + if(angle < 0){continue;} + + const double diffX = dx-sx; + const double diffY = dy-sy; + const double diffZ = dz-sz; + + double probX = kpfunc->getProb(rw*diffX); + double probY = kpfunc->getProb(rw*diffY); + double probZ = kpfunc->getProb(rw*diffZ); + double prob = probX*probY*probZ/(probX*probY*probZ + (1-probX)*(1-probY)*(1-probZ)); + + if(prob < 0.000001){continue;} + + if(visualizationLvl == 5 && i == 0){ + pcl::PointXYZRGBNormal p; + p.x = sx; + p.y = sy; + p.z = sz; + p.normal_x = nx; + p.normal_y = ny; + p.normal_z = nz; + p.b = 0; + p.g = 255; + p.r = 0; + scloud->points.push_back(p); + + pcl::PointXYZRGBNormal pn = p; + pn.x += 0.01*nx; + pn.y += 0.01*ny; + pn.z += 0.01*nz; + + pcl::PointXYZRGBNormal p1; + p1.x = dx; + p1.y = dy; + p1.z = dz; + p1.normal_x = dnx; + p1.normal_y = dny; + p1.normal_z = dnz; + p1.b = 255.0; + p1.g = 0; + p1.r = 0; + dcloud->points.push_back(p1); + + pcl::PointXYZRGBNormal p1n = p1; + p1n.x += 0.01*dnx; + p1n.y += 0.01*dny; + p1n.z += 0.01*dnz; + + char buf [1024]; + sprintf(buf,"line%i",scloud->points.size()); + viewer->addLine(p,p1,1.0-prob,prob,0,buf); + //viewer->addLine(p,p1, angle <= 0 , angle > 0 ,0,buf); + +// sprintf(buf,"slineN%i",scloud->points.size()); +// viewer->addLine(p1,p1n,1,0,1,buf); + +// sprintf(buf,"dlineN%i",scloud->points.size()); +// viewer->addLine(p,pn,1,1,0,buf); + } + + //double weight = kpInfo*prob*rw*rw; + double weight = prob*rw*rw; + wsum += weight; + + wsx += weight * sx; + wsy += weight * sy; + wsz += weight * sz; + + double wsxsx = weight * sx*sx; + double wsysy = weight * sy*sy; + double wszsz = weight * sz*sz; + + ATA.coeffRef (0) += wsysy + wszsz;//a0 * a0; + ATA.coeffRef (1) -= weight * sx*sy;//a0 * a1; + ATA.coeffRef (2) -= weight * sz*sx;//a0 * a2; + + + ATA.coeffRef (7) += wsxsx + wszsz;//a1 * a1; + ATA.coeffRef (8) -= weight * sy*sz;//a1 * a2; + + ATA.coeffRef (14) += wsxsx + wsysy;//a2 * a2; + + ATb.coeffRef (0) += weight * (sy*diffZ -sz*diffY);//a0 * b; + ATb.coeffRef (1) += weight * (-sx*diffZ + sz*diffX);//a1 * b; + ATb.coeffRef (2) += weight * (sx*diffY -sy*diffX);//a2 * b; + ATb.coeffRef (3) += weight * diffX;//a3 * b; + ATb.coeffRef (4) += weight * diffY;//a4 * b; + ATb.coeffRef (5) += weight * diffZ;//a5 * b; + } + + + ATA.coeffRef (4) -= wsz;//a0 * a4; + ATA.coeffRef (9) += wsz;//a1 * a3; + ATA.coeffRef (5) += wsy;//a0 * a5; + ATA.coeffRef (15) -= wsy;//a2 * a3; + ATA.coeffRef (11) -= wsx;//a1 * a5; + ATA.coeffRef (16) += wsx;//a2 * a4; + ATA.coeffRef (21) += wsum;//a3 * a3; + ATA.coeffRef (28) += wsum;//a4 * a4; + ATA.coeffRef (35) += wsum;//a5 * a5; + + ATA.coeffRef (6) = ATA.coeff (1); + ATA.coeffRef (12) = ATA.coeff (2); + ATA.coeffRef (13) = ATA.coeff (8); + ATA.coeffRef (18) = ATA.coeff (3); + ATA.coeffRef (19) = ATA.coeff (9); + ATA.coeffRef (20) = ATA.coeff (15); + ATA.coeffRef (24) = ATA.coeff (4); + ATA.coeffRef (25) = ATA.coeff (10); + ATA.coeffRef (26) = ATA.coeff (16); + ATA.coeffRef (27) = ATA.coeff (22); + ATA.coeffRef (30) = ATA.coeff (5); + ATA.coeffRef (31) = ATA.coeff (11); + ATA.coeffRef (32) = ATA.coeff (17); + ATA.coeffRef (33) = ATA.coeff (23); + ATA.coeffRef (34) = ATA.coeff (29); + + for(int d = 0; d < 6; d++){ + ATA(d,d) += 1; + } + + // Solve A*x = b + Vector6d x = static_cast (ATA.inverse () * ATb); + Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0))); + p = transformation*p; + + double change_t = 0; + double change_r = 0; + for(unsigned int k = 0; k < 3; k++){ + change_t += transformation(k,3)*transformation(k,3); + for(unsigned int l = 0; l < 3; l++){ + if(k == l){ change_r += fabs(1-transformation(k,l));} + else{ change_r += fabs(transformation(k,l));} + } + } + change_t = sqrt(change_t); + + if(visualizationLvl == 5 && i == 0){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + //viewer->addPointCloudNormals(scloud,1,0.2,"scloudn"); + + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + //viewer->addPointCloudNormals(scloud,1,0.2,"dcloudn"); + viewer->spin(); + viewer->removeAllPointClouds(); + viewer->removeAllShapes(); + } + + if(change_t < stopval && change_r < stopval){do_inner = false;} + } + + + //new_opt_time += getTime()-new_opt_start; + //std::cout << p.matrix() << std::endl; + //Eigen::Matrix4d newpose = p.matrix()*poses[i]; + poses[i] = p.matrix();//newpose; + } + }else if(type == PointToPlane && optt == 1){ + for(unsigned int i = 0; i < nr_frames; i++){ + if(getTime()-total_time_start > timeout){break;} + if(!is_ok[i]){continue;} + unsigned int count = 0; + + Eigen::Matrix & tXi = transformed_points[i]; + Eigen::Matrix & Xi = points[i]; + Eigen::Matrix & tXni = transformed_normals[i]; + Eigen::Matrix & Xni = normals[i]; + Eigen::VectorXd & informationi = informations[i]; + + + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + Eigen::Matrix & tXj = transformed_points[j]; + Eigen::Matrix & tXnj = transformed_normals[j]; + Eigen::VectorXd & informationj = informations[j]; + + std::vector & matchidj = matchids[j][i]; + unsigned int matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + std::vector & matchdistj = matchdists[j][i]; + std::vector & matchdisti = matchdists[i][j]; + + + for(unsigned int ki = 0; ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( kj == -1 ){continue;} + if( kj >= matchesj){continue;} + if(!onetoone || matchidj[kj] == ki){ + Qp_arr[3*count+0] = tXj(0,kj); + Qp_arr[3*count+1] = tXj(1,kj); + Qp_arr[3*count+2] = tXj(2,kj); + Qn_arr[3*count+0] = tXnj(0,kj); + Qn_arr[3*count+1] = tXnj(1,kj); + Qn_arr[3*count+2] = tXnj(2,kj); + + Xp_arr[3*count+0] = tXi(0,ki); + Xp_arr[3*count+1] = tXi(1,ki); + Xp_arr[3*count+2] = tXi(2,ki); + Xn_arr[3*count+0] = tXni(0,ki); + Xn_arr[3*count+1] = tXni(1,ki); + Xn_arr[3*count+2] = tXni(2,ki); + + rangeW_arr[count] = 1.0/(1.0/informationi(ki)+1.0/informationj(kj)); + count++; + } + } + } + + if(count == 0){break;} + + typedef Eigen::Matrix Vector6d; + typedef Eigen::Matrix Matrix6d; + + Matrix6d ATA; + Vector6d ATb; + + + Eigen::Affine3d p = Eigen::Affine3d::Identity(); + bool do_inner = true; + for(int inner=0; inner < 5 && do_inner; ++inner) { + ATA.setZero (); + ATb.setZero (); + const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3); + const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3); + const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3); + for(unsigned int co = 0; co < count; co++){ + const double & src_x = Xp_arr[3*co+0]; + const double & src_y = Xp_arr[3*co+1]; + const double & src_z = Xp_arr[3*co+2]; + const double & src_nx = Xn_arr[3*co+0]; + const double & src_ny = Xn_arr[3*co+1]; + const double & src_nz = Xn_arr[3*co+2]; + + const double & dx = Qp_arr[3*co+0]; + const double & dy = Qp_arr[3*co+1]; + const double & dz = Qp_arr[3*co+2]; + const double & dnx = Qn_arr[3*co+0]; + const double & dny = Qn_arr[3*co+1]; + const double & dnz = Qn_arr[3*co+2]; + + const double & rw = rangeW_arr[co]; + + const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; + const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; + const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; + + const double & nx = m00*src_nx + m01*src_ny + m02*src_nz; + const double & ny = m10*src_nx + m11*src_ny + m12*src_nz; + const double & nz = m20*src_nx + m21*src_ny + m22*src_nz; + + if(nx*dnx+ny*dny+nz*dnz < 0){continue;} + + double di = rw*(nx*(sx-dx) + ny*(sy-dy) + nz*(sz-dz)); + double weight = func->getProb(di)*rw*rw; + + const double & a = nz*sy - ny*sz; + const double & b = nx*sz - nz*sx; + const double & c = ny*sx - nx*sy; + + ATA.coeffRef (0) += weight * a * a; + ATA.coeffRef (1) += weight * a * b; + ATA.coeffRef (2) += weight * a * c; + ATA.coeffRef (3) += weight * a * nx; + ATA.coeffRef (4) += weight * a * ny; + ATA.coeffRef (5) += weight * a * nz; + ATA.coeffRef (7) += weight * b * b; + ATA.coeffRef (8) += weight * b * c; + ATA.coeffRef (9) += weight * b * nx; + ATA.coeffRef (10) += weight * b * ny; + ATA.coeffRef (11) += weight * b * nz; + ATA.coeffRef (14) += weight * c * c; + ATA.coeffRef (15) += weight * c * nx; + ATA.coeffRef (16) += weight * c * ny; + ATA.coeffRef (17) += weight * c * nz; + ATA.coeffRef (21) += weight * nx * nx; + ATA.coeffRef (22) += weight * nx * ny; + ATA.coeffRef (23) += weight * nx * nz; + ATA.coeffRef (28) += weight * ny * ny; + ATA.coeffRef (29) += weight * ny * nz; + ATA.coeffRef (35) += weight * nz * nz; + + const double & d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz); + + ATb.coeffRef (0) += a * d; + ATb.coeffRef (1) += b * d; + ATb.coeffRef (2) += c * d; + ATb.coeffRef (3) += nx * d; + ATb.coeffRef (4) += ny * d; + ATb.coeffRef (5) += nz * d; + } + + ATA.coeffRef (6) = ATA.coeff (1); + ATA.coeffRef (12) = ATA.coeff (2); + ATA.coeffRef (13) = ATA.coeff (8); + ATA.coeffRef (18) = ATA.coeff (3); + ATA.coeffRef (19) = ATA.coeff (9); + ATA.coeffRef (20) = ATA.coeff (15); + ATA.coeffRef (24) = ATA.coeff (4); + ATA.coeffRef (25) = ATA.coeff (10); + ATA.coeffRef (26) = ATA.coeff (16); + ATA.coeffRef (27) = ATA.coeff (22); + ATA.coeffRef (30) = ATA.coeff (5); + ATA.coeffRef (31) = ATA.coeff (11); + ATA.coeffRef (32) = ATA.coeff (17); + ATA.coeffRef (33) = ATA.coeff (23); + ATA.coeffRef (34) = ATA.coeff (29); + + // Solve A*x = b + Vector6d x = static_cast (ATA.inverse () * ATb); + Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0))); + p = transformation*p; + + double change_t = 0; + double change_r = 0; + for(unsigned int k = 0; k < 3; k++){ + change_t += transformation(k,3)*transformation(k,3); + for(unsigned int l = 0; l < 3; l++){ + if(k == l){ change_r += fabs(1-transformation(k,l));} + else{ change_r += fabs(transformation(k,l));} + } + } + change_t = sqrt(change_t); + if(change_t < stopval && change_r < stopval){do_inner = false;} + } + + //new_opt_time += getTime()-new_opt_start; + //std::cout << p.matrix() << std::endl; + Eigen::Matrix4d newpose = p.matrix()*poses[i]; + poses[i] = newpose; + } + }else{ + + for(unsigned int i = 0; i < nr_frames; i++){ + if(getTime()-total_time_start > timeout){break;} + if(!is_ok[i]){continue;} + unsigned int nr_match = 0; + { + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + std::vector & matchidj = matchids[j][i]; + unsigned int matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + + std::vector & matchdistj = matchdists[j][i]; + std::vector & matchdisti = matchdists[i][j]; + + + for(unsigned int ki = 0; ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( kj == -1 ){continue;} + if( kj >= matchesj){continue;} + if(!onetoone || matchidj[kj] == ki){ nr_match++;} + } + } + } + + Eigen::Matrix3Xd Xp = Eigen::Matrix3Xd::Zero(3, nr_match); + Eigen::Matrix3Xd Xp_ori = Eigen::Matrix3Xd::Zero(3, nr_match); + Eigen::Matrix3Xd Xn = Eigen::Matrix3Xd::Zero(3, nr_match); + + Eigen::Matrix3Xd Qp = Eigen::Matrix3Xd::Zero(3, nr_match); + Eigen::Matrix3Xd Qn = Eigen::Matrix3Xd::Zero(3, nr_match); + Eigen::VectorXd rangeW = Eigen::VectorXd::Zero( nr_match); + + int count = 0; + + Eigen::Matrix & tXi = transformed_points[i]; + Eigen::Matrix & Xi = points[i]; + Eigen::Matrix & tXni = transformed_normals[i]; + Eigen::Matrix & Xni = normals[i]; + Eigen::VectorXd & informationi = informations[i]; + + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + Eigen::Matrix & tXj = transformed_points[j]; + Eigen::Matrix & tXnj = transformed_normals[j]; + Eigen::VectorXd & informationj = informations[j]; + + std::vector & matchidj = matchids[j][i]; + unsigned int matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + + std::vector & matchdistj = matchdists[j][i]; + std::vector & matchdisti = matchdists[i][j]; + + for(unsigned int ki = 0; ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( kj == -1 ){continue;} + if( kj >= matchesj){continue;} + if(!onetoone || matchidj[kj] == ki){ + Qp.col(count) = tXj.col(kj); + Qn.col(count) = tXnj.col(kj); + + Xp_ori.col(count) = Xi.col(ki); + Xp.col(count) = tXi.col(ki); + + Xn.col(count) = tXni.col(ki); + rangeW(count) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj)); + count++; + } + } + } + + if(count == 0){break;} + + //showMatches(Xp,Qp); + for(int inner=0; inner < 5; ++inner) { + Eigen::MatrixXd residuals; + switch(type) { + case PointToPoint: {residuals = Xp-Qp;} break; + case PointToPlane: { + residuals = Eigen::MatrixXd::Zero(1, Xp.cols()); + for(int i=0; igetProbs(residuals); } break; + case PointToPlane: { + W = func->getProbs(residuals); + for(int k=0; k 0.0);} + } break; + default: {printf("type not set\n");} break; + } + + W = W.array()*rangeW.array()*rangeW.array(); + Xo1 = Xp; + switch(type) { + case PointToPoint: { + //RigidMotionEstimator::point_to_point(Xp, Qp, W); + pcl::TransformationFromCorrespondences tfc1; + for(unsigned int c = 0; c < nr_match; c++){tfc1.add(Eigen::Vector3f(Xp(0,c), Xp(1,c),Xp(2,c)),Eigen::Vector3f(Qp(0,c),Qp(1,c),Qp(2,c)),W(c));} + Eigen::Affine3d rot = tfc1.getTransformation().cast(); + Xp = rot*Xp; + Xn = rot.rotation()*Xn; + } break; + case PointToPlane: { + point_to_plane2(Xp, Xn, Qp, Qn, W); + } break; + default: {printf("type not set\n"); } break; + } + + double stop1 = (Xp-Xo1).colwise().norm().maxCoeff(); + Xo1 = Xp; + if(stop1 < 0.001){break; } + } + //exit(0); + pcl::TransformationFromCorrespondences tfc; + for(unsigned int c = 0; c < nr_match; c++){tfc.add(Eigen::Vector3f(Xp_ori(0,c),Xp_ori(1,c),Xp_ori(2,c)),Eigen::Vector3f(Xp(0,c),Xp(1,c),Xp(2,c)));} + poses[i] = tfc.getTransformation().cast().matrix(); + } + } + + Eigen::Matrix4d p0inv = poses[0].inverse(); + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + poses[j] = p0inv*poses[j]; + + Eigen::Matrix & tXi = transformed_points[j]; + Eigen::Matrix & Xi = points[j]; + Eigen::Matrix & tXni = transformed_normals[j]; + Eigen::Matrix & Xni = normals[j]; + + Eigen::Matrix4d p = poses[j]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + for(int c = 0; c < Xi.cols(); c++){ + float x = Xi(0,c); + float y = Xi(1,c); + float z = Xi(2,c); + + float nx = Xni(0,c); + float ny = Xni(1,c); + float nz = Xni(2,c); + + tXi(0,c) = m00*x + m01*y + m02*z + m03; + tXi(1,c) = m10*x + m11*y + m12*z + m13; + tXi(2,c) = m20*x + m21*y + m22*z + m23; + + tXni(0,c) = m00*nx + m01*ny + m02*nz; + tXni(1,c) = m10*nx + m11*ny + m12*nz; + tXni(2,c) = m20*nx + m21*ny + m22*nz; + } + } + if(isconverged(poses, poses2, stopval, stopval)){break;} + } + +// exit(0); + return poses; +} + +int popcount_lauradoux(uint64_t *buf, uint32_t size) { + const uint64_t* data = (uint64_t*) buf; + + const uint64_t m1 = UINT64_C(0x5555555555555555); + const uint64_t m2 = UINT64_C(0x3333333333333333); + const uint64_t m4 = UINT64_C(0x0F0F0F0F0F0F0F0F); + const uint64_t m8 = UINT64_C(0x00FF00FF00FF00FF); + const uint64_t m16 = UINT64_C(0x0000FFFF0000FFFF); + const uint64_t h01 = UINT64_C(0x0101010101010101); + + uint32_t bitCount = 0; + uint32_t i, j; + uint64_t count1, count2, half1, half2, acc; + uint64_t x; + uint32_t limit30 = size - size % 30; + + // 64-bit tree merging (merging3) + for (i = 0; i < limit30; i += 30, data += 30) { + acc = 0; + for (j = 0; j < 30; j += 3) { + count1 = data[j]; + count2 = data[j+1]; + half1 = data[j+2]; + half2 = data[j+2]; + half1 &= m1; + half2 = (half2 >> 1) & m1; + count1 -= (count1 >> 1) & m1; + count2 -= (count2 >> 1) & m1; + count1 += half1; + count2 += half2; + count1 = (count1 & m2) + ((count1 >> 2) & m2); + count1 += (count2 & m2) + ((count2 >> 2) & m2); + acc += (count1 & m4) + ((count1 >> 4) & m4); + } + acc = (acc & m8) + ((acc >> 8) & m8); + acc = (acc + (acc >> 16)) & m16; + acc = acc + (acc >> 32); + bitCount += (uint32_t)acc; + } + + for (i = 0; i < size - limit30; i++) { + x = data[i]; + x = x - ((x >> 1) & m1); + x = (x & m2) + ((x >> 2) & m2); + x = (x + (x >> 4)) & m4; + bitCount += (uint32_t)((x * h01) >> 56); + } + return bitCount; +} + +void MassRegistrationPPR2::rematchKeyPoints(std::vector poses, std::vector prev_poses, bool first){ + printf("rematchKeyPoints\n"); + double total_rematchKeyPoints_time_start = getTime(); + unsigned int nr_frames = poses.size(); + + int max_kps = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + printf("max_kps: %i kp_nr_arraypoints: %i\n",max_kps, kp_nr_arraypoints[i]); + max_kps = std::max(max_kps,kp_nr_arraypoints[i]); + } + printf("max_kps: %i\n",max_kps); + + if(max_kps == 0){return;} + + double * best_src = new double[max_kps]; + double * best_src2 = new double[max_kps]; + double * best_src_e = new double[max_kps]; + double * best_src_f = new double[max_kps]; + int * best_src_id = new int[max_kps]; + + double * best_dst = new double[max_kps]; + double * best_dst_e = new double[max_kps]; + double * best_dst_f = new double[max_kps]; + int * best_dst_id = new int[max_kps]; + + for(unsigned int i = 0; i < nr_frames; i++){ + kp_matches.resize(nr_frames); + for(unsigned int j = 0; j < nr_frames; j++){ + kp_matches[i].resize(nr_frames); + } + } + + + int ignores = 0; +bool useOrb = true; +if(useOrb){ + for(unsigned int i = 0; i < nr_frames; i++){ + const unsigned int src_nr_ap = kp_nr_arraypoints[i]; + if(src_nr_ap == 0){continue;} + double * src_ap = kp_arraypoints[i]; + double * src_an = kp_arraynormals[i]; + double * src_ai = kp_arrayinformations[i]; + uint64_t * src_data = kp_arraydescriptors[i]; + + for(unsigned int j = 0; j < nr_frames; j++){ + if(i == j){continue;} + + const unsigned int dst_nr_ap = kp_nr_arraypoints[j]; + if(dst_nr_ap == 0){continue;} + + for(int k = 0; k < src_nr_ap; k++){ + best_src[k] = 999999999999; + best_src2[k] = 999999999999; + best_src_id[k] = -1; + } + for(int k = 0; k < dst_nr_ap; k++){ + best_dst[k] = 999999999999; + best_dst_id[k] = -1; + } + + + double * dst_ap = kp_arraypoints[j]; + double * dst_an = kp_arraynormals[j]; + double * dst_ai = kp_arrayinformations[j]; + uint64_t * dst_data = kp_arraydescriptors[j]; + + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + + if(!first){ + Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); + + Eigen::Affine3d diff = prev_rp.inverse()*rp; + + double change_trans = 0; + double change_rot = 0; + double dt = 0; + for(unsigned int k = 0; k < 3; k++){ + dt += diff(k,3)*diff(k,3); + for(unsigned int l = 0; l < 3; l++){ + if(k == l){ change_rot += fabs(1-diff(k,l));} + else{ change_rot += fabs(diff(k,l));} + } + } + change_trans += sqrt(dt); + + if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;} + } + + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + float di = 10.0; + float pi = 10.0; + + #pragma omp parallel for num_threads(8) + for(unsigned int src_k = 0; src_k < src_nr_ap; ++src_k) { + const double & src_x = src_ap[3*src_k+0]; + const double & src_y = src_ap[3*src_k+1]; + const double & src_z = src_ap[3*src_k+2]; + + double sx = m00*src_x + m01*src_y + m02*src_z + m03; + double sy = m10*src_x + m11*src_y + m12*src_z + m13; + double sz = m20*src_x + m21*src_y + m22*src_z + m23; + + unsigned int src_k4 = src_k*4; + const uint64_t s1 = src_data[src_k4+0]; + const uint64_t s2 = src_data[src_k4+1]; + const uint64_t s3 = src_data[src_k4+2]; + const uint64_t s4 = src_data[src_k4+3]; + + uint64_t xordata [4]; + for(unsigned int dst_k = 0; dst_k < dst_nr_ap; ++dst_k) { + double dx = sx-dst_ap[3*dst_k+0]; + double dy = sy-dst_ap[3*dst_k+1]; + double dz = sz-dst_ap[3*dst_k+2]; + + double p_dist = sqrt(dx*dx+dy*dy+dz*dz); + + unsigned int dst_k4 = dst_k*4; + const uint64_t d1 = dst_data[dst_k4+0]; + const uint64_t d2 = dst_data[dst_k4+1]; + const uint64_t d3 = dst_data[dst_k4+2]; + const uint64_t d4 = dst_data[dst_k4+3]; + + xordata[0] = s1 ^ d1; + xordata[1] = s2 ^ d2; + xordata[2] = s3 ^ d3; + xordata[3] = s4 ^ d4; + + int cnt = popcount_lauradoux(xordata, 4); + double f_dist = float(cnt)/256.0f; + + float d = f_dist*di + p_dist*pi; + + if(d < best_src[src_k]){ + + best_src2[src_k] = best_src[src_k]; + best_src_id[src_k] = dst_k; + best_src[src_k] = d; + }else if(d < best_src2[src_k]){ + best_src2[src_k] = d; + } + + + + if(d < best_dst[dst_k]){ + best_dst_id[dst_k] = src_k; + best_dst[dst_k] = d; + } + + } + } + + std::vector< TestMatch > matches; + double threshold = 2.0; + double ratiothreshold = 0.8; + + int nr_matches = 0; + for(unsigned int src_k = 0; src_k < src_nr_ap; ++src_k) { + int dst_k = best_src_id[src_k]; + + if(best_dst_id[dst_k] != src_k){continue;}//One to one + if(best_src[src_k] > threshold){continue;}//threshold + if(best_src[src_k]/best_src2[src_k] < ratiothreshold){continue;}//ratiothreshold + + if(dst_an[3*dst_k+0] > 1 || src_an[3*src_k+0] > 1){continue;} + + nr_matches++; + + matches.push_back(TestMatch(src_k, dst_k,best_src[src_k])); + } + kp_matches[i][j] = matches; + } + } +} + + +bool useSurf = false; +if(useSurf){ + for(unsigned int i = 0; i < nr_frames; i++){ + const unsigned int src_nr_ap = kp_nr_arraypoints[i]; + if(src_nr_ap == 0){continue;} + double * src_ap = kp_arraypoints[i]; + double * src_an = kp_arraynormals[i]; + double * src_ai = kp_arrayinformations[i]; + float* src_data = (float*)(kp_arraydescriptors[i]); + + for(unsigned int j = 0; j < nr_frames; j++){ + if(i == j){continue;} + + const unsigned int dst_nr_ap = kp_nr_arraypoints[j]; + if(dst_nr_ap == 0){continue;} + + for(int k = 0; k < src_nr_ap; k++){ + best_src[k] = 999999999999; + best_src_id[k] = -1; + } + for(int k = 0; k < dst_nr_ap; k++){ + best_dst[k] = 999999999999; + best_dst_id[k] = -1; + } + + + double * dst_ap = kp_arraypoints[j]; + double * dst_an = kp_arraynormals[j]; + double * dst_ai = kp_arrayinformations[j]; + float* dst_data = (float*)(kp_arraydescriptors[j]); + + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + + if(!first){ + Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); + + Eigen::Affine3d diff = prev_rp.inverse()*rp; + + double change_trans = 0; + double change_rot = 0; + double dt = 0; + for(unsigned int k = 0; k < 3; k++){ + dt += diff(k,3)*diff(k,3); + for(unsigned int l = 0; l < 3; l++){ + if(k == l){ change_rot += fabs(1-diff(k,l));} + else{ change_rot += fabs(diff(k,l));} + } + } + change_trans += sqrt(dt); + + if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;} + } + + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + #pragma omp parallel for num_threads(8) + for(unsigned int src_k = 0; src_k < src_nr_ap; ++src_k) { + const double & src_x = src_ap[3*src_k+0]; + const double & src_y = src_ap[3*src_k+1]; + const double & src_z = src_ap[3*src_k+2]; + + double sx = m00*src_x + m01*src_y + m02*src_z + m03; + double sy = m10*src_x + m11*src_y + m12*src_z + m13; + double sz = m20*src_x + m21*src_y + m22*src_z + m23; + + + + for(unsigned int dst_k = 0; dst_k < dst_nr_ap; ++dst_k) { + double dx = sx-dst_ap[3*dst_k+0]; + double dy = sy-dst_ap[3*dst_k+1]; + double dz = sz-dst_ap[3*dst_k+2]; + + + double f_dist = 0; + + for(unsigned int f_k = 0; f_k < 128; ++f_k) { + double diff = src_data[src_k*128 + f_k] - dst_data[dst_k*128 + f_k]; + f_dist += diff*diff; + } + + float d = f_dist;//*di + p_dist*pi; + + if(d < best_src[src_k]){ + best_src_id[src_k] = dst_k; + best_src[src_k] = d; + } + + if(d < best_dst[dst_k]){ + best_dst_id[dst_k] = src_k; + best_dst[dst_k] = d; + } + + } + } + + std::vector< TestMatch > matches; + double threshold = 0.25; + int nr_matches = 0; + for(unsigned int src_k = 0; src_k < src_nr_ap; ++src_k) { + int dst_k = best_src_id[src_k]; + + if(best_dst_id[dst_k] != src_k || best_src[src_k] > threshold){continue;}//One to one + + if(dst_an[3*dst_k+0] > 1 || src_an[3*src_k+0] > 1){continue;} + + nr_matches++; + + matches.push_back(TestMatch(src_k, dst_k,best_src[src_k])); + } + kp_matches[i][j] = matches; + } + } +} +if(false){ + for(unsigned int i = 0; i < nr_frames; i++){ + if(frameid[i] == -1){continue;} + for(unsigned int j = 0; j < nr_frames; j++){ + if(frameid[j] == -1){continue;} + if(kp_matches[i][j].size() == 0){continue;} + unsigned char * src_data = (unsigned char *)(model->frames[frameid[i]]->rgb.data); + unsigned char * dst_data = (unsigned char *)(model->frames[frameid[j]]->rgb.data); + + cv::Mat img; + img.create(480,2*640,CV_8UC3); + unsigned char * imgdata = (unsigned char *)img.data; + for(int w = 0; w < 640; w++){ + for(int h = 0; h < 480; h++){ + int ind = h*640+w; + + int ind2 = h*2*640+w; + int ind3 = h*2*640+w+640; + + imgdata[3*ind2+0] = src_data[3*ind+0]; + imgdata[3*ind2+1] = src_data[3*ind+1]; + imgdata[3*ind2+2] = src_data[3*ind+2]; + + imgdata[3*ind3+0] = dst_data[3*ind+0]; + imgdata[3*ind3+1] = dst_data[3*ind+1]; + imgdata[3*ind3+2] = dst_data[3*ind+2]; + } + } + + std::vector & src_keypoints = model->all_keypoints[i]; + std::vector & dst_keypoints = model->all_keypoints[j]; + + std::vector< TestMatch > matches = kp_matches[i][j]; + for(unsigned int k = 0; k < matches.size(); ++k) { + cv::KeyPoint & spt = src_keypoints[matches[k].src]; + cv::KeyPoint & dpt = dst_keypoints[matches[k].dst]; + + //printf("%i -> ") + + cv::line(img, spt.pt, cv::Point(dpt.pt.x+640,dpt.pt.y), cv::Scalar(255,0,255)); + } + + cv::imshow( "matches", img ); + cv::waitKey(0); + + //for(int j = 0; j < width*height; j++){maskdata[j] = 255*maskvec[j];} + } + } +} + + delete[] best_src; + delete[] best_src_e; + delete[] best_src_f; + delete[] best_src_id; + delete[] best_dst; + delete[] best_dst_e; + delete[] best_dst_f; + delete[] best_dst_id; + + if(visualizationLvl > 0){printf("total rematchKeyPoints time: %5.5f\n",getTime()-total_rematchKeyPoints_time_start);} + +} + +MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses){ + if(visualizationLvl > 0){printf("start MassRegistrationPPR2::getTransforms(std::vector poses)\n");} + + unsigned int nr_frames = informations.size(); + if(poses.size() != nr_frames){ + printf("ERROR: poses.size() = %i != informations.size() %i\n",poses.size(), informations.size()); + return MassFusionResults(); + } + + fast_opt = false; + if(fast_opt){ + printf("debugging... setting nr frames to 3: %s :: %i\n",__FILE__,__LINE__); + nr_frames = 3; + } + + for(unsigned int i = 0; i < nr_frames; i++){ + Eigen::Matrix4d p = poses[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + if(!is_ok[i]){continue;} + + matchids[i].resize(nr_frames); + matchdists[i].resize(nr_frames); + + Eigen::Matrix & X = points[i]; + Eigen::Matrix & Xn = normals[i]; + Eigen::Matrix & tX = transformed_points[i]; + Eigen::Matrix & tXn = transformed_normals[i]; + int count = nr_datas[i]; + for(int c = 0; c < count; c++){ + float x = X(0,c); + float y = X(1,c); + float z = X(2,c); + float xn = Xn(0,c); + float yn = Xn(1,c); + float zn = Xn(2,c); + + tX(0,c) = m00*x + m01*y + m02*z + m03; + tX(1,c) = m10*x + m11*y + m12*z + m13; + tX(2,c) = m20*x + m21*y + m22*z + m23; + tXn(0,c) = m00*xn + m01*yn + m02*zn; + tXn(1,c) = m10*xn + m11*yn + m12*zn; + tXn(2,c) = m20*xn + m21*yn + m22*zn; + } + + } + + func->reset(); + kpfunc->reset(); + + int imgcount = 0; + char buf [1024]; + if(visualizationLvl > 0){ + std::vector Xv; + for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + sprintf(buf,"image%5.5i.png",imgcount++); + show(Xv,false,std::string(buf),imgcount); + } + + rematch_time = 0; + residuals_time = 0; + opt_time = 0; + computeModel_time = 0; + setup_matches_time = 0; + setup_equation_time = 0; + setup_equation_time2 = 0; + solve_equation_time = 0; + total_time_start = getTime(); + + + + bool first = true; + std::vector poses0 = poses; + //rematchKeyPoints(poses,poses0,first); + + for(int funcupdate=0; funcupdate < 100; ++funcupdate) { + if(getTime()-total_time_start > timeout){break;} + if(visualizationLvl == 2){std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + + for(int rematching=0; rematching < 10; ++rematching) { + if(visualizationLvl == 3){std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + std::vector poses1 = poses; + + double rematch_time_start = getTime(); + rematch(poses,poses0,first); + //rematchKeyPoints(poses,poses0,first); + first = false; + poses0 = poses; + rematch_time += getTime()-rematch_time_start; + + for(int lala = 0; lala < 10; lala++){ + if(visualizationLvl > 0){ + printf("funcupdate: %i rematching: %i lala: %i\n",funcupdate,rematching,lala); + printf("total_time: %5.5f\n",getTime()-total_time_start); + printf("rematch_time: %5.5f\n",rematch_time); + printf("compM residuals_time:%5.5f\n",residuals_time); + printf("computeModel: %5.5f\n",computeModel_time); + printf("opt_time: %5.5f\n",opt_time); + } + if(visualizationLvl == 4){std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + std::vector poses2b = poses; + + double residuals_time_start = getTime(); + Eigen::MatrixXd all_residuals = getAllResiduals(poses); + +// Eigen::MatrixXd all_KPresiduals = getAllKpResiduals(poses); + residuals_time += getTime()-residuals_time_start; + + double computeModel_time_start = getTime(); + func->computeModel(all_residuals); +// kpfunc->computeModel(all_KPresiduals); + stopval = func->getNoise()*0.1; +// printf("reg: %f noise:%f stopval: %f\n",func->regularization,func->noiseval,stopval); + +// double stopval1 = func->getNoise()*0.1; +// double stopval2 = kpfunc->getNoise()*0.1; +// if(func->noiseval > 0 && kpfunc->noiseval > 0){ stopval = std::min(stopval1,stopval2);} +// else if(func->noiseval > 0){ stopval = func->getNoise()*0.1;} +// else if(kpfunc->noiseval > 0){ stopval = kpfunc->getNoise()*0.1;} +// else{ break;} +// printf("reg: %f noise:%f stopval: %f\n",func->regularization,func->noiseval,stopval); +// printf("kpreg: %f kpnoise:%f stopval: %f\n",kpfunc->regularization,kpfunc->noiseval,stopval); + + computeModel_time += getTime()-computeModel_time_start; + + double opt_time_start = getTime(); + poses = optimize(poses); + opt_time += getTime()-opt_time_start; + + if(isconverged(poses, poses2b, stopval, stopval)){break;} + } + if(isconverged(poses, poses1, stopval, stopval)){break;} + } + + double noise_before = func->getNoise(); + func->update(); + double noise_after = func->getNoise(); + double ratio = noise_after/noise_before; + + double kpnoise_before = kpfunc->getNoise(); + kpfunc->update(); + double kpnoise_after = kpfunc->getNoise(); + double kpratio = kpnoise_after/kpnoise_before; + + double reg1 = func->regularization; + double reg2 = kpfunc->regularization; + func->regularization = std::max(reg1,reg2); + kpfunc->regularization = std::max(reg1,reg2); + + printf("func->noiseval: %f func->regularization: %f\n",func->noiseval,func->regularization); + //printf("ratio: %f kpratio: %f\n",ratio,kpratio); + //if(func->noiseval >= kpfunc->noiseval && 0.01*func->noiseval < func->regularization){break;} + //if(func->noiseval < kpfunc->noiseval && 0.01*kpfunc->noiseval < kpfunc->regularization){break;} + //if(ratio > 0.99){break;} + if(func->noiseval > 10.0*func->regularization && ratio > 0.75){break;} + } + + printf("total_time: %5.5f\n",getTime()-total_time_start); + printf("rematch_time: %5.5f\n",rematch_time); + printf("compM residuals_time:%5.5f\n",residuals_time); + printf("computeModel: %5.5f\n",computeModel_time); + printf("opt_time: %5.5f\n",opt_time); +// printf("setup_matches_time: %5.5f\n",setup_matches_time); +// printf("setup_equation_time: %5.5f\n",setup_equation_time); +// printf("solve_equation_time: %5.5f\n",solve_equation_time); + + if(visualizationLvl > 0){ + std::vector Xv; + for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + sprintf(buf,"image%5.5i.png",imgcount++); + show(Xv,false,std::string(buf),imgcount); + } + + Eigen::Matrix4d firstinv = poses.front().inverse(); + for(int i = 0; i < nr_frames; i++){poses[i] = firstinv*poses[i];} + + printf("stop MassRegistrationPPR2::getTransforms(std::vector guess)\n"); + //exit(0); + return MassFusionResults(poses,-1); +} + +} diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp index 89e8c4c1..6c798b8d 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp @@ -1,6 +1,7 @@ #include "registration/RegistrationRandom.h" #include #include +#include namespace reglib { @@ -36,6 +37,33 @@ double transformationdiff(Eigen::Affine3d & A, Eigen::Affine3d & B, double rotat return r*rotationweight+t; } + +bool compareFusionResults (FusionResults i,FusionResults j) { return i.score > j.score; } + +bool RegistrationRandom::issame(FusionResults fr1, FusionResults fr2, int stepxsmall){ + Eigen::Matrix4d current_guess = fr1.guess.inverse()*fr2.guess; + float m00 = current_guess(0,0); float m01 = current_guess(0,1); float m02 = current_guess(0,2); float m03 = current_guess(0,3); + float m10 = current_guess(1,0); float m11 = current_guess(1,1); float m12 = current_guess(1,2); float m13 = current_guess(1,3); + float m20 = current_guess(2,0); float m21 = current_guess(2,1); float m22 = current_guess(2,2); float m23 = current_guess(2,3); + + double sum = 0; + double count = 0; + unsigned int s_nr_data = src->data.cols(); + for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++){ + float x = src->data(0,i*stepxsmall); + float y = src->data(1,i*stepxsmall); + float z = src->data(2,i*stepxsmall); + float dx = m00*x + m01*y + m02*z + m03 - x; + float dy = m10*x + m11*y + m12*z + m13 - y; + float dz = m20*x + m21*y + m22*z + m23 - z; + sum += sqrt(dx*dx+dy*dy+dz*dz); + count++; + } + double mean = sum/count; + //printf("mean: %f\n",mean); + return mean < 20*fr1.stop; +} + FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ unsigned int s_nr_data = src->data.cols();//std::min(int(src->data.cols()),int(500000)); @@ -90,8 +118,6 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ d_mean_y /= double(d_nr_data); d_mean_z /= double(d_nr_data); - double stop = 0.00001; - Eigen::Affine3d Ymean = Eigen::Affine3d::Identity(); Ymean(0,3) = d_mean_x; Ymean(1,3) = d_mean_y; @@ -102,11 +128,7 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ Xmean(1,3) = s_mean_y; Xmean(2,3) = s_mean_z; - std::vector< Eigen::Matrix > all_X; - std::vector< Eigen::Affine3d > all_res; - std::vector< int > count_X; - std::vector< float > score_X; - std::vector< std::vector< Eigen::VectorXd > > all_starts; + int stepxsmall = std::max(1,int(s_nr_data)/250); Eigen::VectorXd Wsmall (s_nr_data/stepxsmall); for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++){Wsmall(i) = src->information(0,i*stepxsmall);} @@ -115,136 +137,132 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ double sumtime = 0; double sumtimeSum = 0; double sumtimeOK = 0; - int r = 0; refinement->viewer = viewer; refinement->visualizationLvl = 0; - //for(int r = 0; r < 1000; r++){ -// while(true){ -// double rx = 2.0*M_PI*0.0001*double(rand()%10000); -// double ry = 2.0*M_PI*0.0001*double(rand()%10000); -// double rz = 2.0*M_PI*0.0001*double(rand()%10000); - //double stop = 0; + + std::vector< double > rxs; + std::vector< double > rys; + std::vector< double > rzs; double step = 0.1+2.0*M_PI/5; for(double rx = 0; rx < 2.0*M_PI; rx += step){ - for(double ry = 0; ry < 2.0*M_PI; ry += step) - for(double rz = 0; rz < 2.0*M_PI; rz += step){ - printf("rx: %f ry: %f rz: %f\n",rx,ry,rz); + for(double ry = 0; ry < 2.0*M_PI; ry += step){ + for(double rz = 0; rz < 2.0*M_PI; rz += step){ + rxs.push_back(rx); + rys.push_back(ry); + rzs.push_back(rz); + } + } + } + + refinement->target_points = 250; + unsigned int nr_r = rxs.size(); + + std::vector fr_X; + fr_X.resize(nr_r); + + #pragma omp parallel for num_threads(8) + for(unsigned int r = 0; r < nr_r; r++){ double start = getTime(); double meantime = 999999999999; - if(r != 0){meantime = sumtimeSum/double(sumtimeOK+1.0);} - refinement->maxtime = std::min(0.5,3*meantime); - - Eigen::VectorXd startparam = Eigen::VectorXd(3); - startparam(0) = rx; - startparam(1) = ry; - startparam(2) = rz; + if(sumtimeOK != 0){meantime = sumtimeSum/double(sumtimeOK+1.0);} + refinement->maxtime = std::min(0.5,3*meantime); Eigen::Affine3d randomrot = Eigen::Affine3d::Identity(); - - randomrot = Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()); + randomrot = Eigen::AngleAxisd(rxs[r], Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(rys[r], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rzs[r], Eigen::Vector3d::UnitZ()); Eigen::Affine3d current_guess = Ymean*randomrot*Xmean.inverse();//*Ymean; - refinement->target_points = 250; - FusionResults fr = refinement->getTransform(current_guess.matrix()); -//fr.timeout = timestopped; - double stoptime = getTime(); - sumtime += stoptime-start; - - if(!fr.timeout){ - sumtimeSum += stoptime-start; - sumtimeOK++; - } - //printf("sumtime: %f\n",sumtime); - stop = fr.score; - Eigen::Matrix4d m = fr.guess; - current_guess = m;//fr.guess; - float m00 = current_guess(0,0); float m01 = current_guess(0,1); float m02 = current_guess(0,2); float m03 = current_guess(0,3); - float m10 = current_guess(1,0); float m11 = current_guess(1,1); float m12 = current_guess(1,2); float m13 = current_guess(1,3); - float m20 = current_guess(2,0); float m21 = current_guess(2,1); float m22 = current_guess(2,2); float m23 = current_guess(2,3); - - Eigen::Matrix Xsmall; - Xsmall.resize(Eigen::NoChange,s_nr_data/stepxsmall); - for(unsigned int i = 0; i < s_nr_data/stepxsmall; i++){ - float x = src->data(0,i*stepxsmall); - float y = src->data(1,i*stepxsmall); - float z = src->data(2,i*stepxsmall); - Xsmall(0,i) = m00*x + m01*y + m02*z + m03; - Xsmall(1,i) = m10*x + m11*y + m12*z + m13; - Xsmall(2,i) = m20*x + m21*y + m22*z + m23; - } + FusionResults fr = refinement->getTransform(current_guess.matrix()); + //fr_X[r] = refinement->getTransform(current_guess.matrix()); - bool exists = false; - for(unsigned int ax = 0; ax < all_X.size(); ax++){ - Eigen::Matrix axX = all_X[ax]; - Eigen::Affine3d axT = all_res[ax]; - double diff = (Xsmall-axX).colwise().norm().mean(); - if(diff < 20*stop){ - count_X[ax]++; - all_starts[ax].push_back(startparam); - int count = count_X[ax]; - float score = score_X[ax]; - std::vector< Eigen::VectorXd > starts = all_starts[ax]; - for(int bx = ax-1; bx >= 0; bx--){ - if(count_X[bx] < count_X[bx+1]){ - count_X[bx+1] = count_X[bx]; - score_X[bx+1] = score_X[bx]; - all_X[bx+1] = all_X[bx]; - all_starts[bx+1] = all_starts[bx]; - all_res[bx+1] = all_res[bx]; - - - all_X[bx] = axX; - count_X[bx] = count; - score_X[bx] = score; - all_starts[bx] = starts; - all_res[bx] = axT; - }else{break;} - } - exists = true; - break; - } - } + #pragma omp critical + { + fr_X[r] = fr; - - - if(!exists){ - all_X.push_back(Xsmall); - count_X.push_back(1); - score_X.push_back(stop); - all_starts.push_back(std::vector< Eigen::VectorXd >()); - all_starts.back().push_back(startparam); - all_res.push_back(current_guess); + double stoptime = getTime(); + sumtime += stoptime-start; + if(!fr_X[r].timeout){ + sumtimeSum += stoptime-start; + sumtimeOK++; + } + // bool exists2 = false; + // for(unsigned int ax = 0; ax < fr_X.size(); ax++){ + // if(issame(fr, fr_X[ax],stepxsmall)){exists2 = true; break;} + // } + // if(!exists2){fr_X.push_back(fr);} } - r++; } -} + FusionResults fr = FusionResults(); refinement->allow_regularization = false; int tpbef = refinement->target_points; - refinement->target_points = 2000; - for(unsigned int ax = 0; ax < all_X.size(); ax++){ - Eigen::Matrix4d np = all_res[ax].matrix(); - refinement->visualizationLvl = visualizationLvl; - if(ax < 25){ - double start = getTime(); - FusionResults fr1 = refinement->getTransform(np); - double stop = getTime(); - np = fr1.guess; + + + for(int tp = 500; tp <= 1000; tp *= 2){ + refinement->target_points = tp; + + unsigned int nr_X = fr_X.size(); + #pragma omp parallel for num_threads(8) + for(unsigned int ax = 0; ax < nr_X; ax++){ + fr_X[ax] = refinement->getTransform(fr_X[ax].guess); + } + + for(unsigned int ax = 0; ax < fr_X.size(); ax++){ + for(unsigned int bx = ax+1; bx < fr_X.size(); bx++){ + if(issame(fr_X[bx], fr_X[ax],stepxsmall)){ + fr_X[bx] = fr_X.back(); + fr_X.pop_back(); + bx--; + } + } } - refinement->visualizationLvl = 0; - fr.candidates.push_back(np); - fr.counts.push_back(count_X[ax]); - fr.scores.push_back(1.0/score_X[ax]); } + +// for(int tp = 500; tp <= 1000; tp *= 2){ +// refinement->target_points = tp; + +// unsigned int nr_X = fr_X.size(); +// //#pragma omp parallel for num_threads(8) +// for(unsigned int ax = 0; ax < nr_X; ax++){ +// fr_X[ax] = refinement->getTransform(fr_X[ax].guess); +// } + +// for(unsigned int ax = 0; ax < fr_X.size(); ax++){ +// for(unsigned int bx = ax+1; bx < fr_X.size(); bx++){ +// if(issame(fr_X[bx], fr_X[ax],stepxsmall)){ +// fr_X[bx] = fr_X.back(); +// fr_X.pop_back(); +// bx--; +// } +// } +// } +// } + + refinement->visualizationLvl = visualizationLvl; + std::sort (fr_X.begin(), fr_X.end(), compareFusionResults); + for(unsigned int ax = 0; ax < fr_X.size() && ax < 500; ax++){ + fr.candidates.push_back(fr_X[ax].guess); + fr.counts.push_back(1); + fr.scores.push_back(fr_X[ax].score); + } + + + refinement->visualizationLvl = 2; + refinement->target_points = 30000; + for(unsigned int ax = 0; ax < fr_X.size() && ax < 30; ax++){ + printf("%i -> %f\n",ax,fr_X[ax].score); + refinement->getTransform(fr_X[ax].guess); + } + + refinement->visualizationLvl = 0; refinement->target_points = tpbef; return fr; } diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp index 9c918c85..b5f8930f 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp @@ -29,12 +29,12 @@ RegistrationRefinement::RegistrationRefinement(){ allow_regularization = true; maxtime = 9999999; - func = new DistanceWeightFunction2PPR2(); - func->startreg = 0.1; - func->debugg_print = false; +// func = new DistanceWeightFunction2PPR2(); +// func->startreg = 0.1; +// func->debugg_print = false; } RegistrationRefinement::~RegistrationRefinement(){ - if(func != 0){delete func; func = 0;} + //if(func != 0){delete func; func = 0;} if(arraypoints != 0){delete arraypoints; arraypoints = 0;} if(trees3d != 0){delete trees3d; trees3d = 0;} if(a3d != 0){delete a3d; a3d = 0;} @@ -47,7 +47,7 @@ void RegistrationRefinement::setDst(CloudData * dst_){ Y.resize(Eigen::NoChange,d_nr_data/stepy); N.resize(Eigen::NoChange,d_nr_data/stepy); ycols = Y.cols(); - total_dweight.resize(ycols); + int count = 0; for(unsigned int i = 0; i < d_nr_data/stepy; i++){ @@ -84,9 +84,16 @@ void RegistrationRefinement::setDst(CloudData * dst_){ FusionResults RegistrationRefinement::getTransform(Eigen::MatrixXd guess){ -// DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); -// func->startreg = 0.1; -// func->debugg_print = false; + std::vector total_dweight; + total_dweight.resize(ycols); + + DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + if(allow_regularization){ + func->startreg = 0.1; + }else{ + func->startreg = 0.0; + } + func->debugg_print = false; unsigned int s_nr_data = src->data.cols(); @@ -142,6 +149,8 @@ FusionResults RegistrationRefinement::getTransform(Eigen::MatrixXd guess){ double score = 0; stop = 99999; + //if(visualizationLvl >= 2){show(X,Y);} + double start = getTime(); bool timestopped = false; @@ -151,7 +160,7 @@ bool timestopped = false; for(int rematching=0; rematching < 100; ++rematching) { if( (getTime()-start) > maxtime ){timestopped = true; break;} -#pragma omp parallel for +//#pragma omp parallel for for(unsigned int i=0; i< xcols; ++i) { std::vector ret_indexes(1); std::vector out_dists_sqr(1); @@ -164,7 +173,7 @@ bool timestopped = false; } /// Find closest point -#pragma omp parallel for +//#pragma omp parallel for for(unsigned int i=0; i< xcols; ++i) { int id = matchid[i]; Qn.col(i) = N.col(id); @@ -202,7 +211,7 @@ bool timestopped = false; for(int rematching2=0; rematching2 < 3; ++rematching2) { if( (getTime()-start) > maxtime ){timestopped = true; break;} if(rematching2 != 0){ -#pragma omp parallel for +//#pragma omp parallel for for(unsigned int i=0; i< xcols; ++i) { std::vector ret_indexes(1); std::vector out_dists_sqr(1); @@ -215,7 +224,7 @@ bool timestopped = false; } /// Find closest point -#pragma omp parallel for +//#pragma omp parallel for for(unsigned int i=0; i< xcols; ++i) { int id = matchid[i]; Qn.col(i) = N.col(id); @@ -286,7 +295,9 @@ bool timestopped = false; } stop = 100*func->getConvergenceThreshold(); - score = Wold.sum()/(pow(func->getNoise(),2)*float(xcols)); + //score = Wold.sum()/(pow(func->getNoise(),2)*float(xcols)); + score = Wold.sum()/(func->getNoise()*float(xcols)); + double stop1 = (X-Xo1).colwise().norm().mean(); Xo1 = X; @@ -310,7 +321,7 @@ bool timestopped = false; if(fabs(1.0 - noise_after/noise_before) < 0.01){break;} } - if(visualizationLvl >= 2){show(X,Y);} + if(visualizationLvl >= 2){printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise()); show(X,Y);} pcl::TransformationFromCorrespondences tfc; tfc.reset(); @@ -322,8 +333,12 @@ bool timestopped = false; //delete func; //func = 0; - FusionResults fr = FusionResults(guess,stop); + + if(func != 0){delete func; func = 0;} + + FusionResults fr = FusionResults(guess,score); fr.timeout = timestopped; + fr.stop = stop; return fr; } diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp index a5aa5a2c..950b7f98 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp @@ -648,10 +648,12 @@ bool DistanceWeightFunction2PPR2::update(){ Gaussian3 g = Gaussian3(mulval,meanval,stdval);//getModel(stdval,blur_histogram,uniform_bias); int iteration = 0; - for(int i = 0; i < 100; i++){ + double regularization_before = regularization; + for(int i = 0; i < 500; i++){ iteration++; regularization *= 0.5; double change = histogram_size*regularization/maxd; + //printf("DistanceWeightFunction2PPR2::update() %i -> reg %f change %f\n",i,regularization,change); if(change < 0.01*stdval){return true;} @@ -664,7 +666,8 @@ bool DistanceWeightFunction2PPR2::update(){ new_sum_prob += std::min(maxp , g.getval(k)/hs) * new_histogram[k]; } - if(new_sum_prob < 0.99*old_sum_prob){return false;} + //printf("new_sum_prob %f old_sum_prob %f ratio: %f\n",new_sum_prob,old_sum_prob,new_sum_prob/old_sum_prob); + if(new_sum_prob < 0.99*old_sum_prob || regularization < regularization_before/double(target_length*5)){return false;} g.stdval -= change; g.update(); } From e716b909606c6e0882e851339846e6f94f7a9168 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 1 Jul 2016 14:23:27 +0200 Subject: [PATCH 015/255] removed deps --- quasimodo/quasimodo_models/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 9e33fc9d..2d878583 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include - LIBRARIES quasimodo_ModelUpdater quasimodo_SICP quasimodo_reglib quasimodo_massreglib quasimodo_model quasimodo_core quasimodo_myhull quasimodo_Mesher quasimodo_weightlib quasimodo_modelmask + LIBRARIES quasimodo_ModelUpdater quasimodo_reglib quasimodo_massreglib quasimodo_model quasimodo_core quasimodo_Mesher quasimodo_weightlib quasimodo_modelmask # CATKIN_DEPENDS other_catkin_pkg # DEPENDS system_lib ) From 0d010be24f4f33e16b906c2089e00cf03044ff42 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 1 Jul 2016 14:39:05 +0200 Subject: [PATCH 016/255] removed need for nonfree --- .../quasimodo_models/src/modelupdater/ModelUpdater.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 3d3b3fcf..7cac7481 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -16,7 +16,7 @@ #include "opencv2/features2d/features2d.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/calib3d/calib3d.hpp" -#include "opencv2/nonfree/nonfree.hpp" +//#include "opencv2/nonfree/nonfree.hpp" typedef boost::property edge_weight_property; typedef boost::property vertex_name_property; @@ -604,8 +604,8 @@ void ModelUpdater::makeInitialSetup(){ int minHessian = 400; - cv::SurfFeatureDetector detector( minHessian ); - cv::SurfDescriptorExtractor extractor; +// cv::SurfFeatureDetector detector( minHessian ); +// cv::SurfDescriptorExtractor extractor; //cv::ORB orb = cv::ORB(250);//,1.2f, 1, 3, 0,2, cv::ORB::HARRIS_SCORE, 31); @@ -639,7 +639,7 @@ void ModelUpdater::makeInitialSetup(){ MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); massreg->timeout = 4*massreg_timeout; massreg->viewer = viewer; - massreg->visualizationLvl = 1;//1; + massreg->visualizationLvl = 0;//1; massreg->maskstep = std::max(1,int(0.5+0.2*double(model->frames.size()))); massreg->nomaskstep = std::max(5,int(0.5+1.0*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massreg->nomask = true; @@ -1076,7 +1076,7 @@ printf("%s::%i\n",__FILE__,__LINE__); MassRegistrationPPR * massreg = new MassRegistrationPPR(reg); massreg->timeout = massreg_timeout; massreg->viewer = viewer; - massreg->visualizationLvl = 1; + massreg->visualizationLvl = 0; massreg->maskstep = 4;//std::max(1,int(0.5+0.02*double(model->frames.size()))); massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); printf("maskstep: %i nomaskstep: %i\n",massreg->maskstep,massreg->nomaskstep); From 7343733f9441f8bc1cda0453bb2cebf1a38bcbc3 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 1 Jul 2016 14:46:42 +0200 Subject: [PATCH 017/255] removed recognition service --- quasimodo/quasimodo_brain/src/modelserver.cpp | 90 +------------------ 1 file changed, 1 insertion(+), 89 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 8eb31494..62827017 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -12,7 +12,6 @@ #include "quasimodo_msgs/index_frame.h" #include "quasimodo_msgs/fuse_models.h" #include "quasimodo_msgs/get_model.h" -#include "quasimodo_msgs/recognize_model.h" #include "quasimodo_msgs/retrieval_query_result.h" #include "quasimodo_msgs/retrieval_query.h" #include "quasimodo_msgs/retrieval_result.h" @@ -419,93 +418,6 @@ void publishModel(reglib::Model * model){ // } } -bool recognizeModel(quasimodo_msgs::recognize_model::Request & req, quasimodo_msgs::recognize_model::Response & res){ - if(modeldatabase->models.size() == 0){res.score = -1;return true;} - reglib::Model * model = getModelFromMSG(req.inputmodel); - - double best = 0; - int best_ind = 0; - for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - reglib::Model * model2 = modeldatabase->models[i]; - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model2, reg); - mu->occlusion_penalty = occlusion_penalty; - mu->massreg_timeout = massreg_timeout; - mu->viewer = viewer; - reg->visualizationLvl = 0; - reglib::FusionResults fr = mu->registerModel(model); - - if(fr.score > 100){ - fr.guess = fr.guess.inverse(); - - Eigen::Matrix4d pose = fr.guess; - std::vector current_poses; - std::vector current_frames; - std::vector current_modelmasks; - - for(unsigned int i = 0; i < model->frames.size(); i++){ - current_poses.push_back( model->relativeposes[i]); - current_frames.push_back( model->frames[i]); - current_modelmasks.push_back( model->modelmasks[i]); - } - - for(unsigned int i = 0; i < model2->frames.size(); i++){ - current_poses.push_back( pose * model2->relativeposes[i]); - current_frames.push_back( model2->frames[i]); - current_modelmasks.push_back( model2->modelmasks[i]); - } - - std::vector > ocs = mu->getOcclusionScores(current_poses, current_frames,current_modelmasks,false); - std::vector > scores = mu->getScores(ocs); - - - unsigned int frames1 = model->frames.size(); - unsigned int frames2 = model2->frames.size(); - double sumscore1 = 0; - for(unsigned int i = 0; i < frames1; i++){ - for(unsigned int j = 0; j < frames1; j++){ - sumscore1 += scores[i][j]; - } - } - - double sumscore2 = 0; - for(unsigned int i = 0; i < frames2; i++){ - for(unsigned int j = 0; j < frames2; j++){ - sumscore2 += scores[i+frames1][j+frames1]; - } - } - - double sumscore = 0; - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - sumscore += scores[i][j]; - } - } - - double improvement = sumscore-sumscore1-sumscore2; - if(i == 0 || improvement > best){ - best_ind = i; - best = improvement; - } - } - - delete mu; - delete reg; - } - - for(unsigned int i = 0; i < model->frames.size(); i++){ - delete model->frames[i]; - delete model->modelmasks[i]; - } - delete model; - - res.score = best; - res.outputmodel = getModelMSG(modeldatabase->models[best_ind]); - //int model_id = req.model_id; - //reglib::Model * model = models[model_id]; - //res.model = getModelMSG(model); - return true; -} bool getModel(quasimodo_msgs::get_model::Request & req, quasimodo_msgs::get_model::Response & res){ int model_id = req.model_id; @@ -823,7 +735,7 @@ delete reg; modeldatabase->add(newmodelHolder); addToDB(modeldatabase, newmodelHolder,false); - for(unsigned int i = 0; i < modeldatabase->models.size(); i++){modeldatabase->models[i]->showHistory(viewer);} + //for(unsigned int i = 0; i < modeldatabase->models.size(); i++){modeldatabase->models[i]->showHistory(viewer);} show_sorted(); //exit(0); From 88646660f83407a7b48e8388f022a3f5afbcc7bd Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 1 Jul 2016 14:57:37 +0200 Subject: [PATCH 018/255] removed recognition service --- quasimodo/quasimodo_brain/src/modelserver.cpp | 3 - .../quasimodo_models/include/model/Model.h | 2 + .../quasimodo_models/src/model/Model.cpp | 79 +++++++++++++++++++ 3 files changed, 81 insertions(+), 3 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 62827017..72412da4 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -1065,9 +1065,6 @@ int main(int argc, char **argv){ ros::ServiceServer service4 = n.advertiseService("get_model", getModel); ROS_INFO("Ready to add use get_model."); - ros::ServiceServer service5 = n.advertiseService("recognize_model", recognizeModel); - ROS_INFO("Ready to add use get_model."); - soma2add = n.serviceClient("soma2/insert_objs"); ROS_INFO("Ready to add use soma2add."); diff --git a/quasimodo/quasimodo_models/include/model/Model.h b/quasimodo/quasimodo_models/include/model/Model.h index 2ed9bbb6..0eea6431 100644 --- a/quasimodo/quasimodo_models/include/model/Model.h +++ b/quasimodo/quasimodo_models/include/model/Model.h @@ -100,6 +100,8 @@ class superpoint{ void showHistory(boost::shared_ptr viewer); + std::vector::Ptr> getHistory(); + void addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask); void addAllSuperPoints(vector & spvec, Eigen::Matrix4d pose); void recomputeModelPoints(Eigen::Matrix4d pose = Eigen::Matrix4d::Identity()); diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 77fab06a..e9d16b44 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -276,8 +276,87 @@ void Model::showHistory(boost::shared_ptr vie pcl::transformPointCloud (*cloud2, *tcloud, Eigen::Affine3d(submodels_relativeposes[i]).inverse()); viewer->addPointCloud (tcloud, pcl::visualization::PointCloudColorHandlerRGBField(tcloud), "model"); viewer->spin(); + } +} + +std::vector::Ptr> Model::getHistory(){ +std::vector::Ptr> ret; + for(unsigned int i = 0; i < submodels.size(); i++){ + pcl::PointCloud::Ptr room_cloud (new pcl::PointCloud); + for(unsigned int j = 0; j < submodels[i]->frames.size(); j++){ + bool * maskvec = submodels[i]->modelmasks[j]->maskvec; + unsigned char * rgbdata = (unsigned char *)(submodels[i]->frames[j]->rgb.data); + unsigned short * depthdata = (unsigned short *)(submodels[i]->frames[j]->depth.data); + float * normalsdata = (float *)(submodels[i]->frames[j]->normals.data); + + Camera * camera = submodels[i]->frames[j]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + Eigen::Matrix4d p = submodels[i]->relativeposes[j]; + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(!maskvec[ind]){ + float z = idepth*float(depthdata[ind]); + float nx = normalsdata[3*ind+0]; + + if(z > 0 && nx != 2){ + float ny = normalsdata[3*ind+1]; + float nz = normalsdata[3*ind+2]; + + float x = (w - cx) * z * ifx; + float y = (h - cy) * z * ify; + + pcl::PointXYZRGB po; + po.x = m00*x + m01*y + m02*z + m03; + po.y = m10*x + m11*y + m12*z + m13; + po.z = m20*x + m21*y + m22*z + m23; +// float pnx = m00*nx + m01*ny + m02*nz; +// float pny = m10*nx + m11*ny + m12*nz; +// float pnz = m20*nx + m21*ny + m22*nz; + + po.b = rgbdata[3*ind+0]; + po.g = rgbdata[3*ind+1]; + po.r = rgbdata[3*ind+2]; + + + room_cloud->points.push_back(po); + } + } + } + } + } + + + pcl::PointCloud::Ptr cloud = submodels[i]->getPCLcloud(1, true); + for(unsigned int j = 0; j < cloud->points.size(); j++){ + cloud->points[j].r = 255; + cloud->points[j].g = 0; + cloud->points[j].b = 0; + } + + pcl::PointCloud::Ptr cloud2 = getPCLcloud(1, true); + + pcl::PointCloud::Ptr tcloud (new pcl::PointCloud ()); + pcl::transformPointCloud (*cloud2, *tcloud, Eigen::Affine3d(submodels_relativeposes[i]).inverse()); + + *room_cloud += *cloud; + *room_cloud += *tcloud; + ret.push_back(room_cloud); } + return ret; } void Model::addAllSuperPoints(std::vector & spvec, Eigen::Matrix4d pose){ From b1ce0e56d26cb0630269c0f5b8b9a1856a4360a0 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 1 Jul 2016 15:05:22 +0200 Subject: [PATCH 019/255] publish history topic --- quasimodo/quasimodo_brain/src/modelserver.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 72412da4..09fe14b4 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -73,6 +73,7 @@ ros::Publisher models_deleted_pub; ros::Publisher model_pcd_pub; ros::Publisher database_pcd_pub; +ros::Publisher model_history_pub; ros::Publisher chatter_pub; ros::ServiceClient soma2add; @@ -140,6 +141,16 @@ void publishDatabasePCD(bool original_colors = false){ database_pcd_pub.publish(input); } +void publish_history(std::vector::Ptr> history){ + + for(unsigned int i = 0; i < history.size(); i++){ + sensor_msgs::PointCloud2 input; + pcl::toROSMsg (*history[i],input);//, *transformed_cloud); + input.header.frame_id = "/db_frame"; + database_pcd_pub.publish(input); + } +} + void publishObject(int id){ } @@ -735,7 +746,9 @@ delete reg; modeldatabase->add(newmodelHolder); addToDB(modeldatabase, newmodelHolder,false); - //for(unsigned int i = 0; i < modeldatabase->models.size(); i++){modeldatabase->models[i]->showHistory(viewer);} + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + publish_history(modeldatabase->models[i]->getHistory()); + } show_sorted(); //exit(0); @@ -1072,6 +1085,7 @@ int main(int argc, char **argv){ ROS_INFO("Ready to add recieve search results."); database_pcd_pub = n.advertise("modelserver/databasepcd", 1000); + model_history_pub = n.advertise("modelserver/model_history", 1000); int inputstate = -1; for(int i = 1; i < argc;i++){ From b37770b3c0f5d28f6697ffa809b7cdf1925d26c6 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 1 Jul 2016 15:06:58 +0200 Subject: [PATCH 020/255] removed visualization --- quasimodo/quasimodo_brain/src/modelserver.cpp | 2 +- .../src/registration/RegistrationRandom.cpp | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 09fe14b4..0e12cf24 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -607,7 +607,7 @@ show_sorted(); mu->occlusion_penalty = occlusion_penalty; mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; - reg->visualizationLvl = 2; + reg->visualizationLvl = 0; reglib::FusionResults fr = mu->registerModel(model); if(fr.score > 100){ diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp index 6c798b8d..cd8ac317 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp @@ -255,14 +255,14 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ } - refinement->visualizationLvl = 2; - refinement->target_points = 30000; - for(unsigned int ax = 0; ax < fr_X.size() && ax < 30; ax++){ - printf("%i -> %f\n",ax,fr_X[ax].score); - refinement->getTransform(fr_X[ax].guess); - } +// refinement->visualizationLvl = 2; +// refinement->target_points = 30000; +// for(unsigned int ax = 0; ax < fr_X.size() && ax < 30; ax++){ +// printf("%i -> %f\n",ax,fr_X[ax].score); +// refinement->getTransform(fr_X[ax].guess); +// } - refinement->visualizationLvl = 0; +// refinement->visualizationLvl = 0; refinement->target_points = tpbef; return fr; } From 03eb0d2ee23081bb24be5daa6229aaee758c780e Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 1 Jul 2016 17:24:19 +0200 Subject: [PATCH 021/255] debugging registration --- quasimodo/quasimodo_brain/src/modelserver.cpp | 128 +++++++++++++++++- quasimodo/quasimodo_models/src/core/Util.cpp | 4 + .../src/registration/RegistrationRandom.cpp | 25 ++-- .../registration/RegistrationRefinement.cpp | 12 +- 4 files changed, 156 insertions(+), 13 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 0e12cf24..ad804801 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -147,7 +147,7 @@ void publish_history(std::vector::Ptr> history sensor_msgs::PointCloud2 input; pcl::toROSMsg (*history[i],input);//, *transformed_cloud); input.header.frame_id = "/db_frame"; - database_pcd_pub.publish(input); + model_history_pub.publish(input); } } @@ -868,11 +868,132 @@ vector > ocs = computeOcclusionScore(models,rps); int maxval = 0; int maxind = 0; - int dilation_size = 10; + int dilation_size = 0; cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ); + for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ + //framekeys + + cv_bridge::CvImagePtr ret_image_ptr; + try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + cv_bridge::CvImagePtr ret_mask_ptr; + try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + cv_bridge::CvImagePtr ret_depth_ptr; + try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + cv::Mat rgbimage = ret_image_ptr->image; + cv::Mat maskimage = ret_mask_ptr->image; + cv::Mat depthimage = ret_depth_ptr->image; + + cv::Mat erosion_dst; + cv::erode( maskimage, erosion_dst, element ); + + unsigned short * depthdata = (unsigned short * )depthimage.data; + unsigned char * rgbdata = (unsigned char * )rgbimage.data; + unsigned char * maskdata = (unsigned char * )erosion_dst.data; + int count = 0; + for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ + if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ + count++; + } + } + if(count > maxval){ + maxval = count; + maxind = j; + } + } + + //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ + if(maxval > 100){//Atleast some pixels has to be in the mask... + std::string key = sresult.retrieved_image_paths[i].strings[maxind]; + printf("searchresult key:%s\n",key.c_str()); + if(framekeys.count(key) == 0){ + + int j = maxind; + + cv_bridge::CvImagePtr ret_image_ptr; + try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + cv_bridge::CvImagePtr ret_mask_ptr; + try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + cv_bridge::CvImagePtr ret_depth_ptr; + try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + cv::Mat rgbimage = ret_image_ptr->image; + cv::Mat maskimage = ret_mask_ptr->image; + cv::Mat depthimage = ret_depth_ptr->image; + for (int ii = 0; ii < depthimage.rows; ++ii) { + for (int jj = 0; jj < depthimage.cols; ++jj) { + depthimage.at(ii, jj) *= 5; + } + } + + + cv::Mat erosion_dst; + cv::erode( maskimage, erosion_dst, element ); + + // cv::Mat overlap = rgbimage.clone(); + // unsigned short * depthdata = (unsigned short * )depthimage.data; + // unsigned char * rgbdata = (unsigned char * )rgbimage.data; + // unsigned char * maskdata = (unsigned char * )maskimage.data; + // unsigned char * overlapdata = (unsigned char * )overlap.data; + // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ + // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ + // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; + // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; + // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; + // }else{ + // overlapdata[3*pixel+0] = 0; + // overlapdata[3*pixel+1] = 0; + // overlapdata[3*pixel+2] = 0; + // } + // } + // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); + // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); + // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); + // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); + + Eigen::Affine3d epose = Eigen::Affine3d::Identity(); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); + frame->keyval = key; + +// cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); +// frame->show(true); + //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); + reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); + bool res = searchmodel->testFrame(0); + + reglib::Model * searchmodelHolder = new reglib::Model(); + searchmodelHolder->submodels.push_back(searchmodel); + searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + searchmodelHolder->last_changed = ++current_model_update; + searchmodelHolder->recomputeModelPoints(); + + //searchmodelHolder->showHistory(viewer); + + + // if(cv::waitKey(0) != 'n'){ + //Todo:: check new model is not flat or L shape + + printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); + //addToDB(modeldatabase, searchmodel,false,true); + addToDB(modeldatabase, searchmodelHolder,false,true); + show_sorted(); + } + } + + +/* for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ //framekeys @@ -967,6 +1088,7 @@ vector > ocs = computeOcclusionScore(models,rps); Eigen::Affine3d epose = Eigen::Affine3d::Identity(); reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); frame->keyval = key; + frame->show(true); //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); bool res = searchmodel->testFrame(0); @@ -987,7 +1109,9 @@ vector > ocs = computeOcclusionScore(models,rps); show_sorted(); // } } + } + */ } break; diff --git a/quasimodo/quasimodo_models/src/core/Util.cpp b/quasimodo/quasimodo_models/src/core/Util.cpp index b7253caa..1838ef27 100644 --- a/quasimodo/quasimodo_models/src/core/Util.cpp +++ b/quasimodo/quasimodo_models/src/core/Util.cpp @@ -151,8 +151,12 @@ namespace reglib{ ATA.coeffRef (33) = ATA.coeff (23); ATA.coeffRef (34) = ATA.coeff (29); + for(int k = 0; k < 6; k++){ + ATA(k,k) += 1; + } // Solve A*x = b Vector6d x = static_cast (ATA.inverse () * ATb); + //Vector6d x = static_cast (ATA.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(ATb)); Eigen::Affine3d transformation = Eigen::Affine3d(constructTransformationMatrix(x(0,0),x(1,0),x(2,0),x(3,0),x(4,0),x(5,0))); X = transformation*X; diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp index cd8ac317..56a8c219 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp @@ -65,6 +65,10 @@ bool RegistrationRandom::issame(FusionResults fr1, FusionResults fr2, int stepxs } FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ + //refinement->visualizationLvl = visualizationLvl; +// refinement->viewer = viewer; +// refinement->visualizationLvl = 2; + unsigned int s_nr_data = src->data.cols();//std::min(int(src->data.cols()),int(500000)); unsigned int d_nr_data = dst->data.cols(); @@ -162,13 +166,13 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ std::vector fr_X; fr_X.resize(nr_r); - #pragma omp parallel for num_threads(8) + //#pragma omp parallel for num_threads(8) for(unsigned int r = 0; r < nr_r; r++){ double start = getTime(); double meantime = 999999999999; if(sumtimeOK != 0){meantime = sumtimeSum/double(sumtimeOK+1.0);} - refinement->maxtime = std::min(0.5,3*meantime); + refinement->maxtime = std::min(0.5,3*meantime); Eigen::Affine3d randomrot = Eigen::Affine3d::Identity(); randomrot = Eigen::AngleAxisd(rxs[r], Eigen::Vector3d::UnitX()) * @@ -180,7 +184,7 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ FusionResults fr = refinement->getTransform(current_guess.matrix()); //fr_X[r] = refinement->getTransform(current_guess.matrix()); - #pragma omp critical + //#pragma omp critical { fr_X[r] = fr; @@ -255,14 +259,15 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ } -// refinement->visualizationLvl = 2; -// refinement->target_points = 30000; -// for(unsigned int ax = 0; ax < fr_X.size() && ax < 30; ax++){ -// printf("%i -> %f\n",ax,fr_X[ax].score); -// refinement->getTransform(fr_X[ax].guess); -// } + refinement->visualizationLvl = 2; + refinement->target_points = 1000; + for(unsigned int ax = 0; ax < fr_X.size() && ax < 50; ax++){ + printf("%i -> %f\n",ax,fr_X[ax].score); + refinement->getTransform(fr_X[ax].guess); + } + refinement->visualizationLvl = 0; + -// refinement->visualizationLvl = 0; refinement->target_points = tpbef; return fr; } diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp index b5f8930f..403c6ef6 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp @@ -151,6 +151,8 @@ FusionResults RegistrationRefinement::getTransform(Eigen::MatrixXd guess){ //if(visualizationLvl >= 2){show(X,Y);} + //printf("X: %i %i Y: %i %i\n",X.cols(), X.rows(),Y.cols(), Y.rows()); + double start = getTime(); bool timestopped = false; @@ -294,11 +296,17 @@ bool timestopped = false; default: {printf("type not set\n"); } break; } - stop = 100*func->getConvergenceThreshold(); + + + + stop = 0.1*func->getNoise();//100*func->getConvergenceThreshold(); //score = Wold.sum()/(pow(func->getNoise(),2)*float(xcols)); score = Wold.sum()/(func->getNoise()*float(xcols)); +// printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise()); +// printf("X: %i %i Y: %i %i\n",X.cols(), X.rows(),Y.cols(), Y.rows()); +// show(X,Y); double stop1 = (X-Xo1).colwise().norm().mean(); Xo1 = X; if(stop1 < stop) break; @@ -321,6 +329,8 @@ bool timestopped = false; if(fabs(1.0 - noise_after/noise_before) < 0.01){break;} } + //printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise()); + if(visualizationLvl >= 2){printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise()); show(X,Y);} pcl::TransformationFromCorrespondences tfc; From f671a29eb768b62d7e8053c513d60d4e56f0fc75 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Fri, 1 Jul 2016 17:59:47 +0200 Subject: [PATCH 022/255] Added visualization of the raw images --- .../src/quasimodo_retrieval_node.cpp | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index a6811366..1309dc75 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -45,6 +45,7 @@ class retrieval_node { public: ros::NodeHandle n; ros::Publisher pub; + ros::Publisher img_pub; ros::ServiceServer service; ros::Publisher keypoint_pub; ros::Subscriber sub; @@ -116,6 +117,7 @@ class retrieval_node { server.setCallback(boost::bind(&retrieval_node::parameters_callback, this, _1, _2)); pub = n.advertise(retrieval_output, 1); + img_pub = n.advertise("/quasimodo_retrieval/raw_visualization", 1, true); keypoint_pub = n.advertise("/models/keypoints", 1); sub = n.subscribe(retrieval_input, 10, &retrieval_node::run_retrieval, this); service = n.advertiseService("/query_normal_cloud", &retrieval_node::service_callback, this); @@ -403,6 +405,8 @@ class retrieval_node { } res.result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices); + sensor_msgs::Image img_msg = construct_results_image(images); + img_pub.publish(img_msg); cout << "Finished retrieval..." << endl; } @@ -519,6 +523,8 @@ class retrieval_node { tf::transformTFToMsg(room_transform, result.query.room_transform); result.result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices); pub.publish(result); + sensor_msgs::Image img_msg = construct_results_image(images); + img_pub.publish(img_msg); cout << "Finished retrieval..." << endl; } @@ -547,6 +553,26 @@ class retrieval_node { ros_image = *cv_pub_ptr->toImageMsg(); } + sensor_msgs::Image construct_results_image(const vector >& images) + { + int width = 640; + int height = 480; + + pair sizes = make_pair(2, 5); + + cv::Mat visualization = cv::Mat::zeros(height*sizes.first, width*sizes.second, CV_8UC3); + for (size_t i = 0; i < images.size(); ++i) { + size_t offset_height = i / sizes.second; + size_t offset_width = i % sizes.second; + images[i][0].copyTo(visualization(cv::Rect(offset_width*width, offset_height*height, width, height))); + } + + sensor_msgs::Image img_msg; + convert_to_img_msg(visualization, img_msg); + + return img_msg; + } + quasimodo_msgs::retrieval_result construct_msgs(const vector& clouds, const vector, Eigen::aligned_allocator >& initial_poses, const vector >& images, From 003c2604fcf90a0899d04bfe5f1d9d31ce717992 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Sat, 2 Jul 2016 22:27:18 +0200 Subject: [PATCH 023/255] Added a new node for visualizing the mask point clouds sent to johan --- quasimodo/quasimodo_retrieval/CMakeLists.txt | 10 +- .../quasimodo_visualize_retrieval_cloud.cpp | 151 ++++++++++++++++++ 2 files changed, 160 insertions(+), 1 deletion(-) create mode 100644 quasimodo/quasimodo_retrieval/src/quasimodo_visualize_retrieval_cloud.cpp diff --git a/quasimodo/quasimodo_retrieval/CMakeLists.txt b/quasimodo/quasimodo_retrieval/CMakeLists.txt index e1b93d01..71bc0070 100644 --- a/quasimodo/quasimodo_retrieval/CMakeLists.txt +++ b/quasimodo/quasimodo_retrieval/CMakeLists.txt @@ -175,6 +175,7 @@ add_executable(quasimodo_retrieval_server src/quasimodo_retrieval_server.cpp) add_executable(quasimodo_retrieval_publisher src/quasimodo_retrieval_publisher.cpp) add_executable(quasimodo_visualization_server src/quasimodo_visualization_server.cpp) add_executable(quasimodo_visualize_model src/quasimodo_visualize_model.cpp) +add_executable(quasimodo_visualize_retrieval_cloud src/quasimodo_visualize_retrieval_cloud.cpp) add_executable(quasimodo_retrieval_node src/quasimodo_retrieval_node.cpp) add_executable(quasimodo_retrieve_observation src/quasimodo_retrieve_observation.cpp) add_executable(quasimodo_logging_server src/quasimodo_logging_server.cpp) @@ -222,6 +223,13 @@ target_link_libraries(quasimodo_visualize_model ${catkin_LIBRARIES} ) +target_link_libraries(quasimodo_visualize_retrieval_cloud + ${OpenCV_LIBS} + ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} + ${PCL_LIBRARIES} + ${catkin_LIBRARIES} +) + #target_link_libraries(quasimodo_retrieval_publisher # pfhrgb_estimation extract_sift sift # benchmark_result benchmark_retrieval benchmark_visualization benchmark_overlap @@ -281,7 +289,7 @@ install( # Mark executables and/or libraries for installation install(TARGETS quasimodo_retrieval_server quasimodo_retrieval_publisher quasimodo_visualization_server - quasimodo_visualize_model quasimodo_retrieval_node quasimodo_retrieve_observation quasimodo_logging_server + quasimodo_visualize_model quasimodo_visualize_retrieval_cloud quasimodo_retrieval_node quasimodo_retrieve_observation quasimodo_logging_server ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_visualize_retrieval_cloud.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_visualize_retrieval_cloud.cpp new file mode 100644 index 00000000..821133be --- /dev/null +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_visualize_retrieval_cloud.cpp @@ -0,0 +1,151 @@ +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "ros/ros.h" +#include "quasimodo_msgs/retrieval_query_result.h" +#include "quasimodo_msgs/model.h" +#include +#include +#include +#include +#include + +using namespace std; + +using PointT = pcl::PointXYZRGB; +using CloudT = pcl::PointCloud; +using HistT = pcl::Histogram<250>; +using HistCloudT = pcl::PointCloud; +using LabelT = semantic_map_load_utilties::LabelledData; + +POINT_CLOUD_REGISTER_POINT_STRUCT (HistT, + (float[N], histogram, histogram) +) + +class retrieval_cloud_visualizer { +public: + ros::NodeHandle n; + ros::Subscriber sub; + ros::Publisher pubs[10]; + + string input; + + retrieval_cloud_visualizer(const std::string& name) + { + ros::NodeHandle pn("~"); + + pn.param("input", input, std::string("/retrieval_result")); + + sub = n.subscribe(input, 1, &retrieval_cloud_visualizer::callback, this); + + for (size_t i = 0; i < 10; ++i) { + pubs[i] = n.advertise(string("/retrieval_cloud/") + to_string(i), 1, true); + } + } + + CloudT::Ptr construct_cloud(cv::Mat& rgb, cv::Mat& depth, cv::Mat& mask, const Eigen::Matrix3f& K, const Eigen::Matrix4f& T) + { + int height = rgb.rows; + int width = rgb.cols; + + CloudT::Ptr cloud(new CloudT); + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { + if (mask.at(i, j) == 0) { + continue; + } + float d = 0.0002f*float(depth.at(i, j)); + Eigen::Vector3f ep(float(j), float(i), 1.0f); + ep = K.inverse()*ep; + ep = d/ep(2)*ep; + PointT p; + p.getVector3fMap() = ep; + cv::Vec3b colors = rgb.at(i, j); + p.r = colors[0]; p.g = colors[1]; p.b = colors[2]; + cloud->push_back(p); + } + } + CloudT::Ptr temp_cloud(new CloudT); + pcl::transformPointCloud(*cloud, *temp_cloud, T); + return temp_cloud; + } + + void callback(const quasimodo_msgs::retrieval_query_result& result) + { + int m = result.result.retrieved_clouds.size(); // 10! + + for (int j = 0; j < m; ++j) { + int n = result.result.retrieved_images[j].images.size(); + CloudT::Ptr cloud(new CloudT); + + for (int i = 0; i < n; ++i) { + Eigen::Matrix3f K; // = Eigen::Map(cvK.val).cast(); + K << 525.0f, 0.0f, 319.5f, 0.0f, 525.0f, 239.5f, 0.0f, 0.0f, 1.0f; + + Eigen::Affine3d e; + tf::poseMsgToEigen(result.result.retrieved_initial_poses[j].poses[i], e); + Eigen::Matrix4f T = e.matrix().cast(); + + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(result.result.retrieved_images[j].images[j], sensor_msgs::image_encodings::BGR8); + } + catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + exit(-1); + } + + cv_bridge::CvImagePtr cv_depth_ptr; + try { + cv_depth_ptr = cv_bridge::toCvCopy(result.result.retrieved_depths[j].images[j], sensor_msgs::image_encodings::MONO16); + } + catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + exit(-1); + } + + cv_bridge::CvImagePtr cv_mask_ptr; + try { + cv_mask_ptr = cv_bridge::toCvCopy(result.result.retrieved_masks[j].images[j], sensor_msgs::image_encodings::MONO8); + } + catch (cv_bridge::Exception& e) { + ROS_ERROR("cv_bridge exception: %s", e.what()); + exit(-1); + } + + CloudT::Ptr frame_cloud = construct_cloud(cv_ptr->image, cv_depth_ptr->image, cv_mask_ptr->image, K, T); + *cloud += *frame_cloud; + } + + sensor_msgs::PointCloud2 msg_cloud; + pcl::toROSMsg(*cloud, msg_cloud); + msg_cloud.header.stamp = ros::Time::now(); + msg_cloud.header.frame_id = "/map"; + //tf::transformTFToMsg(room_transform, res.query.room_transform); + pubs[j].publish(msg_cloud); + + } + } + +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "quasimodo_visualize_retrieval_cloud"); + + retrieval_cloud_visualizer vrc(ros::this_node::getName()); + + ros::spin(); + + return 0; +} + From 6532697a8fd7da28ecd55032394b79810ae974f8 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 4 Jul 2016 13:25:14 +0200 Subject: [PATCH 024/255] Fixed some of the mask problems --- .../src/quasimodo_retrieval_node.cpp | 21 +++++++++++++++++-- .../quasimodo_visualize_retrieval_cloud.cpp | 14 ++++++------- 2 files changed, 26 insertions(+), 9 deletions(-) diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index a6811366..48e4f4e9 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -47,6 +47,7 @@ class retrieval_node { ros::Publisher pub; ros::ServiceServer service; ros::Publisher keypoint_pub; + ros::Publisher pubs[10]; ros::Subscriber sub; dynamic_reconfigure::Server server; @@ -117,6 +118,9 @@ class retrieval_node { pub = n.advertise(retrieval_output, 1); keypoint_pub = n.advertise("/models/keypoints", 1); + for (size_t i = 0; i < 10; ++i) { + pubs[i] = n.advertise(string("/retrieval_cloud/") + to_string(i), 1, true); + } sub = n.subscribe(retrieval_input, 10, &retrieval_node::run_retrieval, this); service = n.advertiseService("/query_normal_cloud", &retrieval_node::service_callback, this); /* @@ -151,6 +155,7 @@ class retrieval_node { for (int i = 0; i < transforms.size(); ++i) { cv::Mat mask = cv::Mat::zeros(height, width, CV_8UC1); int sum = 0; + pair intermediate_images = sweep_get_rgbd_at(sweep_xml, i); for (const PointT& p : cloud->points) { Eigen::Vector4f q = transforms[i]*p.getVector4fMap(); if (q(2)/q(3) < 0) { @@ -162,6 +167,12 @@ class retrieval_node { if (x >= width || x < 0 || y >= height || y < 0) { continue; } + float depth = 0.001f*float(intermediate_images.second.at(y, x)); + //cout << "Depth: " << depth << endl; + //cout << "Point depth: " << q(2)/q(3) << endl; + if (fabs(depth - q(2)) > 0.02f) { + continue; + } mask.at(y, x) = 255; ++sum; } @@ -178,7 +189,6 @@ class retrieval_node { get<0>(images).push_back(mask); - pair intermediate_images = sweep_get_rgbd_at(sweep_xml, i); get<1>(images).push_back(intermediate_images.first); get<2>(images).push_back(intermediate_images.second); //get<1>(images).push_back(benchmark_retrieval::sweep_get_rgb_at(sweep_xml, i)); @@ -426,7 +436,8 @@ class retrieval_node { cam_model.fromCameraInfo(query_msg->camera); cv::Matx33d cvK = cam_model.intrinsicMatrix(); Eigen::Matrix3f K = Eigen::Map(cvK.val).cast(); - K << 525.0f, 0.0f, 319.5f, 0.0f, 525.0f, 239.5f, 0.0f, 0.0f, 1.0f; // should be removed if Johan returns K? + K << 528.0f, 0.0f, 317.0f, 0.0f, 525.0f, 245.0f, 0.0f, 0.0f, 1.0f; // surfelize_it intrinsics + // K << 525.0f, 0.0f, 319.5f, 0.0f, 525.0f, 239.5f, 0.0f, 0.0f, 1.0f; // should be removed if Johan returns K? HistCloudT::Ptr features(new HistCloudT); CloudT::Ptr keypoints(new CloudT); @@ -518,6 +529,12 @@ class retrieval_node { tf::transformTFToMsg(room_transform, result.query.room_transform); result.result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices); + for (int i = 0; i < retrieved_clouds.size(); ++i) { + sensor_msgs::PointCloud2 cloud_msg = result.result.retrieved_clouds[i]; + cloud_msg.header.stamp = ros::Time::now(); + cloud_msg.header.frame_id = "/map"; + pubs[i].publish(cloud_msg); + } pub.publish(result); cout << "Finished retrieval..." << endl; diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_visualize_retrieval_cloud.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_visualize_retrieval_cloud.cpp index 821133be..627dbeb9 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_visualize_retrieval_cloud.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_visualize_retrieval_cloud.cpp @@ -48,7 +48,7 @@ class retrieval_cloud_visualizer { sub = n.subscribe(input, 1, &retrieval_cloud_visualizer::callback, this); for (size_t i = 0; i < 10; ++i) { - pubs[i] = n.advertise(string("/retrieval_cloud/") + to_string(i), 1, true); + pubs[i] = n.advertise(string("/retrieval_raw_cloud/") + to_string(i), 1, true); } } @@ -63,14 +63,14 @@ class retrieval_cloud_visualizer { if (mask.at(i, j) == 0) { continue; } - float d = 0.0002f*float(depth.at(i, j)); + float d = 0.001f*float(depth.at(i, j)); Eigen::Vector3f ep(float(j), float(i), 1.0f); ep = K.inverse()*ep; ep = d/ep(2)*ep; PointT p; p.getVector3fMap() = ep; cv::Vec3b colors = rgb.at(i, j); - p.r = colors[0]; p.g = colors[1]; p.b = colors[2]; + p.r = colors[2]; p.g = colors[1]; p.b = colors[0]; cloud->push_back(p); } } @@ -81,7 +81,7 @@ class retrieval_cloud_visualizer { void callback(const quasimodo_msgs::retrieval_query_result& result) { - int m = result.result.retrieved_clouds.size(); // 10! + const int m = 10;//result.result.retrieved_clouds.size(); // 10! for (int j = 0; j < m; ++j) { int n = result.result.retrieved_images[j].images.size(); @@ -97,7 +97,7 @@ class retrieval_cloud_visualizer { cv_bridge::CvImagePtr cv_ptr; try { - cv_ptr = cv_bridge::toCvCopy(result.result.retrieved_images[j].images[j], sensor_msgs::image_encodings::BGR8); + cv_ptr = cv_bridge::toCvCopy(result.result.retrieved_images[j].images[i], sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); @@ -106,7 +106,7 @@ class retrieval_cloud_visualizer { cv_bridge::CvImagePtr cv_depth_ptr; try { - cv_depth_ptr = cv_bridge::toCvCopy(result.result.retrieved_depths[j].images[j], sensor_msgs::image_encodings::MONO16); + cv_depth_ptr = cv_bridge::toCvCopy(result.result.retrieved_depths[j].images[i], sensor_msgs::image_encodings::MONO16); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); @@ -115,7 +115,7 @@ class retrieval_cloud_visualizer { cv_bridge::CvImagePtr cv_mask_ptr; try { - cv_mask_ptr = cv_bridge::toCvCopy(result.result.retrieved_masks[j].images[j], sensor_msgs::image_encodings::MONO8); + cv_mask_ptr = cv_bridge::toCvCopy(result.result.retrieved_masks[j].images[i], sensor_msgs::image_encodings::MONO8); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); From 4aae53e8ab1beaca09dc58b7e4280aba9b1490c3 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 5 Jul 2016 00:44:57 +0200 Subject: [PATCH 025/255] Added the fixed camera parameters and camera parameter optimizatioN --- .../include/strands_sweep_registration/pair3DError.h | 3 ++- strands_sweep_registration/src/ProblemFrameConnection.cpp | 6 ++++-- strands_sweep_registration/src/RobotContainer.cpp | 8 +++++++- strands_sweep_registration/src/pair3DError.cpp | 8 +++++--- 4 files changed, 18 insertions(+), 7 deletions(-) diff --git a/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h b/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h index 9aae4815..238ce2cd 100644 --- a/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h +++ b/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h @@ -22,6 +22,7 @@ using ceres::Solve; class pair3DError : public SizedCostFunction<3, 6, 6, 4> { public: + static bool optimizeCameraParams; pair3DError(double sw, double sh, double sz,double dw, double dh, double dz, double weight); ~pair3DError(); bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const ; @@ -34,7 +35,7 @@ class pair3DError : public SizedCostFunction<3, 6, 6, 4> { double dh; double dz; double weight; - bool optimizeCameraParams; + //bool optimizeCameraParams; double information; }; #endif diff --git a/strands_sweep_registration/src/ProblemFrameConnection.cpp b/strands_sweep_registration/src/ProblemFrameConnection.cpp index 84c5b006..b9194654 100644 --- a/strands_sweep_registration/src/ProblemFrameConnection.cpp +++ b/strands_sweep_registration/src/ProblemFrameConnection.cpp @@ -30,8 +30,10 @@ void ProblemFrameConnection::addMatchesToProblem(ceres::Problem & problem, float double sz = src->keypoint_depth.at(src_kp_id); double dz = dst->keypoint_depth.at(dst_kp_id); - CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight); - problem.AddResidualBlock(err, NULL, src_variable, dst_variable, params); + //CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight/pow(sz*sz+dz*dz,2)); + CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight); + problem.AddResidualBlock(err, 0, src_variable, dst_variable, params); + problem.AddResidualBlock(err, new ceres::HuberLoss(0.1), src_variable, dst_variable, params); } } diff --git a/strands_sweep_registration/src/RobotContainer.cpp b/strands_sweep_registration/src/RobotContainer.cpp index 41a27a37..39786904 100644 --- a/strands_sweep_registration/src/RobotContainer.cpp +++ b/strands_sweep_registration/src/RobotContainer.cpp @@ -118,6 +118,10 @@ RobotContainer::~RobotContainer(){ void RobotContainer::initializeCamera(double fx, double fy, double cx, double cy, unsigned int w, unsigned int h) { + fx = 535; + fy = fx; + cx = double(w-1.0)/2.0; + cy = double(h-1.0)/2.0; std::cout<<"Initializing camera with parameters "< getMatchesRansac(std::vector< ProblemFrameConnection * > & pc_vec, float weight = 1, float threshold = 0.005, int ransac_iter = 200000, int nr_points = 3){ +std::vector< CostFunction * > getMatchesRansac(std::vector< ProblemFrameConnection * > & pc_vec, float weight = 1, float threshold = 0.015, int ransac_iter = 200000, int nr_points = 3){ std::vector owner; std::vector match_id; std::vector< Eigen::Vector3f > src_points; @@ -430,6 +434,8 @@ std::vector RobotContainer::runInitialTraining(){ } } + Solve(options, &problem, &summary); + pair3DError::optimizeCameraParams = true; Solve(options, &problem, &summary); //std::cout << summary.FullReport() << "\n"; diff --git a/strands_sweep_registration/src/pair3DError.cpp b/strands_sweep_registration/src/pair3DError.cpp index 99532379..05d26e32 100644 --- a/strands_sweep_registration/src/pair3DError.cpp +++ b/strands_sweep_registration/src/pair3DError.cpp @@ -3,10 +3,12 @@ int sumid = 0; +bool pair3DError::optimizeCameraParams = false; + pair3DError::pair3DError(double sw, double sh, double sz,double dw, double dh, double dz, double weight) : sw(sw), sh(sh), sz(sz), dw(dw), dh(dh), dz(dz), weight(weight) \ { id = sumid++; - optimizeCameraParams = false; + //optimizeCameraParams = true; information = 1.0/1.5; } @@ -42,8 +44,8 @@ bool pair3DError::Evaluate(double const* const* parameters, double* residuals, d double cy = CameraParameters::get().cy(); if(optimizeCameraParams){ - invfx = params[0]; - invfy = params[1]; + invfx = fabs(params[0]); + invfy = fabs(params[1]); cx = params[2]; cy = params[3]; } From bbfbb23c49e60f45e64e4de1c6ea8e0dd84d55fc Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 5 Jul 2016 09:53:16 +0200 Subject: [PATCH 026/255] Restored johans changes --- quasimodo/quasimodo_brain/src/modelserver.cpp | 70 +++++++++++++++---- .../src/preload_object_data.cpp | 4 +- .../quasimodo_brain/src/robot_listener.cpp | 14 ++++ .../quasimodo_models/include/model/Model.h | 6 +- .../include/modelupdater/ModelUpdater.h | 3 + .../src/modelupdater/ModelUpdater.cpp | 55 ++++++++++++++- .../modelupdater/ModelUpdaterBasicFuse.cpp | 2 +- .../src/registration/RegistrationRandom.cpp | 18 ++--- 8 files changed, 143 insertions(+), 29 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index ad804801..bc26b5d3 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -74,6 +74,8 @@ ros::Publisher models_deleted_pub; ros::Publisher model_pcd_pub; ros::Publisher database_pcd_pub; ros::Publisher model_history_pub; +ros::Publisher model_places_pub; +ros::Publisher model_last_pub; ros::Publisher chatter_pub; ros::ServiceClient soma2add; @@ -151,6 +153,25 @@ void publish_history(std::vector::Ptr> history } } +void publish_places(reglib::Model * mod){ + pcl::PointCloud::Ptr cloud = mod->getPCLcloud(1,true); + for(unsigned int i = 0; i < mod->submodels.size(); i++){ + Eigen::Matrix4d pose = mod->submodels[i]->frames.front()->pose * mod->submodels_relativeposes[i].inverse(); + pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); + pcl::transformPointCloud (*cloud, *transformed_cloud, pose); + + + sensor_msgs::PointCloud2 input; + pcl::toROSMsg (*transformed_cloud,input);//, *transformed_cloud); + input.header.frame_id = "/map"; + model_places_pub.publish(input); + } +/* + for(unsigned int i = 0; i < history.size(); i++){ + } + */ +} + void publishObject(int id){ } @@ -722,6 +743,7 @@ bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_ res.model_id = newmodel->id; if(isnewmodel != 0){ + reglib::RGBDFrame * frontframe = newmodel->frames.front(); int current_model_update_before = current_model_update; newmodel->recomputeModelPoints(); @@ -745,10 +767,14 @@ delete reg; newmodelHolder->recomputeModelPoints(); modeldatabase->add(newmodelHolder); - addToDB(modeldatabase, newmodelHolder,false); - for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - publish_history(modeldatabase->models[i]->getHistory()); - } + addToDB(modeldatabase, newmodelHolder,false); + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + publish_history(modeldatabase->models[i]->getHistory()); + } + /* + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + publish_history(modeldatabase->models[i]->getHistory()); + }*/ show_sorted(); //exit(0); @@ -958,17 +984,17 @@ vector > ocs = computeOcclusionScore(models,rps); // overlapdata[3*pixel+2] = 0; // } // } - // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); - // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); - // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); - // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); +// cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); +// cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); +// cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); +// cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); Eigen::Affine3d epose = Eigen::Affine3d::Identity(); reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); frame->keyval = key; -// cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); -// frame->show(true); +// cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); +// frame->show(true); //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); bool res = searchmodel->testFrame(0); @@ -1124,6 +1150,24 @@ vector > ocs = computeOcclusionScore(models,rps); //exit(0); } + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + for(unsigned int j = 0; j < modeldatabase->models[i]->submodels.size(); j++){ + if( modeldatabase->models[i]->submodels[j]->frames.front() == frontframe){ + Eigen::Matrix4d pose = frontframe->pose * modeldatabase->models[i]->submodels_relativeposes[j].inverse(); + + pcl::PointCloud::Ptr cloud = modeldatabase->models[i]->submodels[j]->getPCLcloud(1,true); + pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); + pcl::transformPointCloud (*cloud, *transformed_cloud, pose); + + + sensor_msgs::PointCloud2 input; + pcl::toROSMsg (*transformed_cloud,input);//, *transformed_cloud); + input.header.frame_id = "/map"; + model_last_pub.publish(input); + } + } + } + newmodel = 0; sweepid_counter++; } @@ -1208,8 +1252,10 @@ int main(int argc, char **argv){ ros::Subscriber sub = n.subscribe("/retrieval_result", 1, retrievalCallback); ROS_INFO("Ready to add recieve search results."); - database_pcd_pub = n.advertise("modelserver/databasepcd", 1000); - model_history_pub = n.advertise("modelserver/model_history", 1000); + database_pcd_pub = n.advertise("modelserver/databasepcd", 1000); + model_history_pub = n.advertise("modelserver/model_history", 1000); + model_last_pub = n.advertise("modelserver/last", 1000); + model_places_pub = n.advertise("modelserver/model_places", 1000); int inputstate = -1; for(int i = 1; i < argc;i++){ diff --git a/quasimodo/quasimodo_brain/src/preload_object_data.cpp b/quasimodo/quasimodo_brain/src/preload_object_data.cpp index 1a0a396b..db32da70 100644 --- a/quasimodo/quasimodo_brain/src/preload_object_data.cpp +++ b/quasimodo/quasimodo_brain/src/preload_object_data.cpp @@ -346,7 +346,7 @@ pcl::PointCloud::Ptr getNC(pcl::PointCloud viewrgbs; std::vector viewdepths; diff --git a/quasimodo/quasimodo_brain/src/robot_listener.cpp b/quasimodo/quasimodo_brain/src/robot_listener.cpp index 0895a8b7..0321fdb2 100644 --- a/quasimodo/quasimodo_brain/src/robot_listener.cpp +++ b/quasimodo/quasimodo_brain/src/robot_listener.cpp @@ -99,6 +99,8 @@ std::vector< std::vector > masks; std::vector< std::vector >tfs; std::vector< std::vector > initposes; +ros::Publisher metaroom_pub; + std::vector getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms){ std::vector toRet; ifstream in(poses_file); @@ -127,12 +129,24 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg) ROS_INFO("I heard: [%s]", msg->data.c_str()); + std::string str = msg->data.c_str(); + ObjectData object = semantic_map_load_utilties::loadDynamicObjectFromSingleSweep(msg->data.c_str()); printf("number of inds: %i",int(object.objectScanIndices.size())); printf("AVs: %i\n",int(object.vAdditionalViews.size())); if (!object.vAdditionalViews.size()){return;} + std::cout << "Splitting: " << str << '\n'; + std::size_t found = str.find_last_of("/\\"); + std::cout << " path: " << str.substr(0,found) << '\n'; + std::cout << " file: " << str.substr(found+1) << '\n'; + + //SimpleXMLParser parser; + //SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(str.substr(0,found)+"/room.xml"); + + //roomData.completeRoomCloud(); + std::vector viewrgbs; diff --git a/quasimodo/quasimodo_models/include/model/Model.h b/quasimodo/quasimodo_models/include/model/Model.h index 0eea6431..4353745a 100644 --- a/quasimodo/quasimodo_models/include/model/Model.h +++ b/quasimodo/quasimodo_models/include/model/Model.h @@ -50,7 +50,7 @@ class superpoint{ point = weight*p.point_information*p.point + point_information*point; normal = weight*p.point_information*p.normal + point_information*normal; - feature = weight*p.feature_information*p.feature + feature_information*feature; + //feature = weight*p.feature_information*p.feature + feature_information*feature; normal.normalize(); @@ -58,8 +58,8 @@ class superpoint{ point /= newpweight; point_information = newpweight; - feature /= newfweight; - feature_information = newfweight; + //feature /= newfweight; + //feature_information = newfweight; last_update_frame_id = std::max(p.last_update_frame_id,last_update_frame_id); } diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 7c2d4283..2fb4c68d 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -82,6 +82,9 @@ namespace reglib virtual void addSuperPoints(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int type = 1, bool debugg = false); virtual vector getSuperPoints(vector cp, vector cf, vector cm, int type = 1, bool debugg = false); + virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); + + virtual OcclusionScore computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1, bool debugg = false); virtual OcclusionScore computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp = Matrix4d::Identity(), int step = 1, bool debugg = false); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 7cac7481..bf0c183f 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -639,7 +639,7 @@ void ModelUpdater::makeInitialSetup(){ MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); massreg->timeout = 4*massreg_timeout; massreg->viewer = viewer; - massreg->visualizationLvl = 0;//1; + massreg->visualizationLvl = 1;//1; massreg->maskstep = std::max(1,int(0.5+0.2*double(model->frames.size()))); massreg->nomaskstep = std::max(5,int(0.5+1.0*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massreg->nomask = true; @@ -1216,6 +1216,57 @@ void ModelUpdater::recomputeScores(){ } } +void ModelUpdater::computeOcclusionAreas(vector cp, vector cf, vector cm){ + + //double ** weights_old = new double*[]; + std::vector weights_old; + double ** wo = new double*[cf.size()]; + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + wo[i] = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + wo[i][j] = 1; + } + } + + weights_old.push_back(wo); + + double ** overlaps = new double*[cf.size()]; + double ** total = new double*[cf.size()]; + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + overlaps[i] = new double[nr_pixels]; + total[i] = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + overlaps[i][j] = 1; + total[i][j] = 1; + } + } + + + + + + for(unsigned int iter = 0; iter < 5; iter++){ + + for(unsigned int i = 0; i < cf.size(); i++){ + for(unsigned int j = 0; j < cf.size(); j++){ + //weightedocclusioncounts + } + } + + wo = new double*[cf.size()]; + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + wo[i] = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + wo[i][j] = 1; + } + } + weights_old.push_back(wo); + } +} + OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * src_modelmask, RGBDFrame * dst, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step, bool debugg){ OcclusionScore oc; @@ -1460,7 +1511,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * DistanceWeightFunction2 * func = new DistanceWeightFunction2(); func->f = THRESHOLD; - func->p = 0.005; + func->p = 0.01; Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp index 0997affb..c33a1a3b 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp @@ -161,7 +161,7 @@ UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, int step = 0.5 + expectedCost/(10.0*11509168.5);// ~10 sec predicted max time step = std::max(1,step); - vector > ocs = computeOcclusionScore(models,rps,step,false); + vector > ocs = computeOcclusionScore(models,rps,step,false); std::vector > scores = getScores(ocs); std::vector partition = getPartition(scores,2,5,2); diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp index 56a8c219..9de8edd7 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp @@ -166,7 +166,7 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ std::vector fr_X; fr_X.resize(nr_r); - //#pragma omp parallel for num_threads(8) + //#pragma omp parallel for num_threads(8) for(unsigned int r = 0; r < nr_r; r++){ double start = getTime(); @@ -184,7 +184,7 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ FusionResults fr = refinement->getTransform(current_guess.matrix()); //fr_X[r] = refinement->getTransform(current_guess.matrix()); - //#pragma omp critical + //#pragma omp critical { fr_X[r] = fr; @@ -259,13 +259,13 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ } - refinement->visualizationLvl = 2; - refinement->target_points = 1000; - for(unsigned int ax = 0; ax < fr_X.size() && ax < 50; ax++){ - printf("%i -> %f\n",ax,fr_X[ax].score); - refinement->getTransform(fr_X[ax].guess); - } - refinement->visualizationLvl = 0; +// refinement->visualizationLvl = 2; +// refinement->target_points = 1000; +// for(unsigned int ax = 0; ax < fr_X.size() && ax < 50; ax++){ +// printf("%i -> %f\n",ax,fr_X[ax].score); +// refinement->getTransform(fr_X[ax].guess); +// } +// refinement->visualizationLvl = 0; refinement->target_points = tpbef; From 58febe84a6c0e09182cea80d84cd7455727efd7b Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 5 Jul 2016 13:15:37 +0200 Subject: [PATCH 027/255] fixed sweep registration issues --- .../include/strands_sweep_registration/pair3DError.h | 3 ++- strands_sweep_registration/src/ProblemFrameConnection.cpp | 6 ++++-- strands_sweep_registration/src/RobotContainer.cpp | 6 ++++++ strands_sweep_registration/src/pair3DError.cpp | 8 +++++--- 4 files changed, 17 insertions(+), 6 deletions(-) diff --git a/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h b/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h index 9aae4815..238ce2cd 100644 --- a/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h +++ b/strands_sweep_registration/include/strands_sweep_registration/pair3DError.h @@ -22,6 +22,7 @@ using ceres::Solve; class pair3DError : public SizedCostFunction<3, 6, 6, 4> { public: + static bool optimizeCameraParams; pair3DError(double sw, double sh, double sz,double dw, double dh, double dz, double weight); ~pair3DError(); bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const ; @@ -34,7 +35,7 @@ class pair3DError : public SizedCostFunction<3, 6, 6, 4> { double dh; double dz; double weight; - bool optimizeCameraParams; + //bool optimizeCameraParams; double information; }; #endif diff --git a/strands_sweep_registration/src/ProblemFrameConnection.cpp b/strands_sweep_registration/src/ProblemFrameConnection.cpp index 84c5b006..b9194654 100644 --- a/strands_sweep_registration/src/ProblemFrameConnection.cpp +++ b/strands_sweep_registration/src/ProblemFrameConnection.cpp @@ -30,8 +30,10 @@ void ProblemFrameConnection::addMatchesToProblem(ceres::Problem & problem, float double sz = src->keypoint_depth.at(src_kp_id); double dz = dst->keypoint_depth.at(dst_kp_id); - CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight); - problem.AddResidualBlock(err, NULL, src_variable, dst_variable, params); + //CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight/pow(sz*sz+dz*dz,2)); + CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight); + problem.AddResidualBlock(err, 0, src_variable, dst_variable, params); + problem.AddResidualBlock(err, new ceres::HuberLoss(0.1), src_variable, dst_variable, params); } } diff --git a/strands_sweep_registration/src/RobotContainer.cpp b/strands_sweep_registration/src/RobotContainer.cpp index 41a27a37..5a6b5080 100644 --- a/strands_sweep_registration/src/RobotContainer.cpp +++ b/strands_sweep_registration/src/RobotContainer.cpp @@ -118,6 +118,10 @@ RobotContainer::~RobotContainer(){ void RobotContainer::initializeCamera(double fx, double fy, double cx, double cy, unsigned int w, unsigned int h) { + fx = 533.796412;//535; + fy = 533.112736;//fx; + cx = 314.863333;//double(w-1.0)/2.0; + cy = 241.271340;//double(h-1.0)/2.0; std::cout<<"Initializing camera with parameters "< RobotContainer::runInitialTraining(){ } Solve(options, &problem, &summary); +// pair3DError::optimizeCameraParams = true; +// Solve(options, &problem, &summary); //std::cout << summary.FullReport() << "\n"; //1st forward Y loop diff --git a/strands_sweep_registration/src/pair3DError.cpp b/strands_sweep_registration/src/pair3DError.cpp index 99532379..05d26e32 100644 --- a/strands_sweep_registration/src/pair3DError.cpp +++ b/strands_sweep_registration/src/pair3DError.cpp @@ -3,10 +3,12 @@ int sumid = 0; +bool pair3DError::optimizeCameraParams = false; + pair3DError::pair3DError(double sw, double sh, double sz,double dw, double dh, double dz, double weight) : sw(sw), sh(sh), sz(sz), dw(dw), dh(dh), dz(dz), weight(weight) \ { id = sumid++; - optimizeCameraParams = false; + //optimizeCameraParams = true; information = 1.0/1.5; } @@ -42,8 +44,8 @@ bool pair3DError::Evaluate(double const* const* parameters, double* residuals, d double cy = CameraParameters::get().cy(); if(optimizeCameraParams){ - invfx = params[0]; - invfy = params[1]; + invfx = fabs(params[0]); + invfy = fabs(params[1]); cx = params[2]; cy = params[3]; } From b37cfd3ee5702b8e6c4172e69222260e975c575b Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 5 Jul 2016 22:52:06 +0200 Subject: [PATCH 028/255] changes --- .../quasimodo_brain/src/robot_listener.cpp | 11 +- .../include/modelupdater/ModelUpdater.h | 2 + .../src/modelupdater/ModelUpdater.cpp | 182 +++++++++++++++++- .../modelupdater/ModelUpdaterBasicFuse.cpp | 4 +- .../src/registration/RegistrationRandom.cpp | 19 +- 5 files changed, 199 insertions(+), 19 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/robot_listener.cpp b/quasimodo/quasimodo_brain/src/robot_listener.cpp index 0321fdb2..f816598d 100644 --- a/quasimodo/quasimodo_brain/src/robot_listener.cpp +++ b/quasimodo/quasimodo_brain/src/robot_listener.cpp @@ -155,7 +155,7 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg) std::vector viewtfs; std::vector viewposes; - int step = std::max(1,int(0.5+double(object.vAdditionalViews.size())/10.0)); + int step = 1;//std::max(1,int(0.5+double(object.vAdditionalViews.size())/10.0)); printf("step: %i\n",step); for (unsigned int i=0; i < 2000 && i & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int type = 1, bool debugg = false); virtual vector getSuperPoints(vector cp, vector cf, vector cm, int type = 1, bool debugg = false); + virtual void getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2); + virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index bf0c183f..3aece887 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -323,7 +323,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M DistanceWeightFunction2 * func = new DistanceWeightFunction2(); func->f = THRESHOLD; - func->p = 0.01; + func->p = 0.03; Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} @@ -639,7 +639,7 @@ void ModelUpdater::makeInitialSetup(){ MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); massreg->timeout = 4*massreg_timeout; massreg->viewer = viewer; - massreg->visualizationLvl = 1;//1; + massreg->visualizationLvl = 0;//1; massreg->maskstep = std::max(1,int(0.5+0.2*double(model->frames.size()))); massreg->nomaskstep = std::max(5,int(0.5+1.0*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massreg->nomask = true; @@ -1216,6 +1216,182 @@ void ModelUpdater::recomputeScores(){ } } +void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2){ +/* + unsigned char * src_maskdata = (unsigned char *)(mask->mask.data); + unsigned char * src_rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame->depth.data); + float * src_normalsdata = (float *)(frame->normals.data); + + Camera * src_camera = frame->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + std::vector test_x; + std::vector test_y; + std::vector test_z; + std::vector test_nx; + std::vector test_ny; + std::vector test_nz; + + std::vector & testw = mask->testw; + std::vector & testh = mask->testh; + + for(unsigned int ind = 0; ind < testw.size();ind++){ + unsigned int src_w = testw[ind]; + unsigned int src_h = testh[ind]; + + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] == 255){ + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + test_x.push_back(src_x); + test_y.push_back(src_y); + test_z.push_back(src_z); + test_nx.push_back(src_nx); + test_ny.push_back(src_ny); + test_nz.push_back(src_nz); + } + } + } + + bool debugg = false; + unsigned int nrdata_start = test_x.size(); + + for(unsigned int c = 0; c < cp.size();c++){ + RGBDFrame* dst = cf[c]; + if(dst == frame){continue;} + ModelMask* dst_modelmask = cm[c]; + unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); + float * dst_normalsdata = (float *)(dst->normals.data); + + Matrix4d rp = cp[c].inverse()*p; + + float m00 = rp(0,0); float m01 = rp(0,1); float m02 = rp(0,2); float m03 = rp(0,3); + float m10 = rp(1,0); float m11 = rp(1,1); float m12 = rp(1,2); float m13 = rp(1,3); + float m20 = rp(2,0); float m21 = rp(2,1); float m22 = rp(2,2); float m23 = rp(2,3); + + Camera * dst_camera = dst->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; +pcl::PointCloud::Ptr cloud (new pcl::PointCloud); +if(debugg){ + viewer->removeAllPointClouds(); + + pcl::PointCloud::Ptr cloud2 = dst->getPCLcloud(); + viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud2"); +} + + for(unsigned int ind = 0; ind < test_x.size();ind++){ + float src_nx = test_nx[ind]; + float src_ny = test_ny[ind]; + float src_nz = test_nz[ind]; + + float src_x = test_x[ind]; + float src_y = test_y[ind]; + float src_z = test_z[ind]; + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + pcl::PointXYZRGBNormal point; + if(debugg){ + point.x = tx; + point.y = ty; + point.z = tz; + point.r = 255; + point.g = 0; + point.b = 0; + } + + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + double dst_noise = dst_z * dst_z; + double point_noise = src_z * src_z; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; + + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE + //if(d < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + test_nx[ind] = test_nx.back(); test_nx.pop_back(); + test_ny[ind] = test_ny.back(); test_ny.pop_back(); + test_nz[ind] = test_nz.back(); test_nz.pop_back(); + test_x[ind] = test_x.back(); test_x.pop_back(); + test_y[ind] = test_y.back(); test_y.pop_back(); + test_z[ind] = test_z.back(); test_z.pop_back(); + ind--; +if(debugg){ + point.r = 0; + point.g = 255; + point.b = 0; +} + } + } + } +if(debugg){ + cloud->points.push_back(point); +} + } + +if(debugg){ + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); +} + } + return double(test_x.size())/double(nrdata_start); +*/ +} + void ModelUpdater::computeOcclusionAreas(vector cp, vector cf, vector cm){ //double ** weights_old = new double*[]; @@ -1511,7 +1687,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * DistanceWeightFunction2 * func = new DistanceWeightFunction2(); func->f = THRESHOLD; - func->p = 0.01; + func->p = 0.03; Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp index c33a1a3b..0d10bfe1 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp @@ -68,7 +68,7 @@ printf("expectedCost: %f\n",expectedCost); unsigned int nr_models1 = models.size(); addModelsToVector(models,rps,model2,pose); - vector > ocs = computeOcclusionScore(models,rps,step,false); + vector > ocs = computeOcclusionScore(models,rps,step,false); std::vector > scores = getScores(ocs); std::vector partition = getPartition(scores,2,5,2); @@ -161,7 +161,7 @@ UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, int step = 0.5 + expectedCost/(10.0*11509168.5);// ~10 sec predicted max time step = std::max(1,step); - vector > ocs = computeOcclusionScore(models,rps,step,false); + vector > ocs = computeOcclusionScore(models,rps,step,true); std::vector > scores = getScores(ocs); std::vector partition = getPartition(scores,2,5,2); diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp index 9de8edd7..f9cfa5dd 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp @@ -166,8 +166,9 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ std::vector fr_X; fr_X.resize(nr_r); - //#pragma omp parallel for num_threads(8) + #pragma omp parallel for num_threads(8) for(unsigned int r = 0; r < nr_r; r++){ + printf("registering: %i / %i\n",r+1,nr_r); double start = getTime(); double meantime = 999999999999; @@ -184,7 +185,7 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ FusionResults fr = refinement->getTransform(current_guess.matrix()); //fr_X[r] = refinement->getTransform(current_guess.matrix()); - //#pragma omp critical + #pragma omp critical { fr_X[r] = fr; @@ -259,13 +260,13 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ } -// refinement->visualizationLvl = 2; -// refinement->target_points = 1000; -// for(unsigned int ax = 0; ax < fr_X.size() && ax < 50; ax++){ -// printf("%i -> %f\n",ax,fr_X[ax].score); -// refinement->getTransform(fr_X[ax].guess); -// } -// refinement->visualizationLvl = 0; +// refinement->visualizationLvl = 2; +// refinement->target_points = 1000; +// for(unsigned int ax = 0; ax < fr_X.size() && ax < 15; ax++){ +// printf("%i -> %f\n",ax,fr_X[ax].score); +// refinement->getTransform(fr_X[ax].guess); +// } +// refinement->visualizationLvl = 0; refinement->target_points = tpbef; From 561be3d059f7b408f448064610972a54ca9f50c9 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 6 Jul 2016 13:18:34 +0200 Subject: [PATCH 029/255] added visualization commandline interface --- quasimodo/quasimodo_brain/src/modelserver.cpp | 54 ++++++++++++++---- .../include/modelupdater/ModelUpdater.h | 4 ++ quasimodo/quasimodo_models/src/core/Util.cpp | 1 + .../src/modelupdater/ModelUpdater.cpp | 55 +++++++++++++++++-- .../modelupdater/ModelUpdaterBasicFuse.cpp | 10 +++- .../src/registration/RegistrationRandom.cpp | 24 ++++---- 6 files changed, 121 insertions(+), 27 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index bc26b5d3..f8993b4f 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -51,6 +51,11 @@ #include bool visualization = false; +bool show_db = false;//Full database show +int show_init_lvl = 0;//init show +int show_refine_lvl = 0;//refine show +int show_reg_lvl = 0;//registration show +bool show_scoring = false;//fuse scoring show std::map cameras; @@ -210,7 +215,7 @@ void retrievalCallback(const quasimodo_msgs::retrieval_query_result & qr){ int savecounter = 0; void show_sorted(){ - return; + if(!show_db){return;} if(!visualization){return;} std::vector results; for(unsigned int i = 0; i < modeldatabase->models.size(); i++){results.push_back(modeldatabase->models[i]);} @@ -519,6 +524,7 @@ std::vector fr_res; void call_from_thread(int i) { reglib::Model * model2 = res[i]; + printf("testreg %i to %i\n",int(mod->id),int(model2->id)); reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model2, reg); @@ -527,6 +533,7 @@ void call_from_thread(int i) { mu->viewer = viewer; reg->visualizationLvl = 0; + reglib::FusionResults fr = mu->registerModel(mod); fr_res[i] = fr; @@ -534,6 +541,9 @@ void call_from_thread(int i) { delete reg; } +/* +bool show_db = false;//Full database show +*/ int current_model_update = 0; void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, bool deleteIfFail = false){ @@ -547,6 +557,9 @@ void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, b mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; reg->visualizationLvl = 0; + mu->show_init_lvl = show_init_lvl;//init show + mu->show_refine_lvl = show_refine_lvl;//refine show + mu->show_scoring = show_scoring;//fuse scoring show mu->refine(0.001,false,0); delete mu; delete reg; @@ -628,7 +641,12 @@ show_sorted(); mu->occlusion_penalty = occlusion_penalty; mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; - reg->visualizationLvl = 0; + + mu->show_init_lvl = show_init_lvl;//init show + mu->show_refine_lvl = show_refine_lvl;//refine show + mu->show_scoring = show_scoring;//fuse scoring show + reg->visualizationLvl = show_reg_lvl; + reglib::FusionResults fr = mu->registerModel(model); if(fr.score > 100){ @@ -754,6 +772,11 @@ bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_ mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; + mu->show_init_lvl = show_init_lvl;//init show + mu->show_refine_lvl = show_refine_lvl;//refine show + mu->show_scoring = show_scoring;//fuse scoring show + reg->visualizationLvl = show_reg_lvl; + newmodel->print(); mu->makeInitialSetup(); newmodel->print(); @@ -1273,8 +1296,13 @@ int main(int argc, char **argv){ else if(std::string(argv[i]).compare("-occlusion_penalty") == 0){printf("occlusion_penalty input state\n");inputstate = 4;} else if(std::string(argv[i]).compare("-massreg_timeout") == 0){printf("massreg_timeout input state\n");inputstate = 5;} else if(std::string(argv[i]).compare("-search") == 0){printf("pointcloud search input state\n");run_search = true; inputstate = 6;} - else if(std::string(argv[i]).compare("-v") == 0){ printf("visualization turned on\n"); visualization = true;} - else if(inputstate == 1){ + else if(std::string(argv[i]).compare("-v") == 0){ printf("visualization turned on\n"); visualization = true;} + else if(std::string(argv[i]).compare("-v_init") == 0){ printf("visualization of init turned on\n"); visualization = true; inputstate = 8;} + else if(std::string(argv[i]).compare("-v_refine") == 0 || std::string(argv[i]).compare("-v_ref") == 0){ printf("visualization refinement turned on\n"); visualization = true; inputstate = 9;} + else if(std::string(argv[i]).compare("-v_register") == 0 || std::string(argv[i]).compare("-v_reg") == 0){ printf("visualization registration turned on\n"); visualization = true; inputstate = 10;} + else if(std::string(argv[i]).compare("-v_scoring") == 0 || std::string(argv[i]).compare("-v_score") == 0 || std::string(argv[i]).compare("-v_sco") == 0){ printf("visualization scoring turned on\n"); visualization = true; show_scoring = true;} + else if(std::string(argv[i]).compare("-v_db") == 0){ printf("visualization db turned on\n"); visualization = true; show_db = true;} + else if(inputstate == 1){ reglib::Camera * cam = reglib::Camera::load(std::string(argv[i])); delete cameras[0]; cameras[0] = cam; @@ -1291,12 +1319,18 @@ int main(int argc, char **argv){ occlusion_penalty = atof(argv[i]); printf("occlusion_penalty set to %f\n",occlusion_penalty); }else if(inputstate == 5){ massreg_timeout = atof(argv[i]); printf("massreg_timeout set to %f\n",massreg_timeout); - }else if(inputstate == 6){ - search_timeout = atof(argv[i]); printf("search_timeout set to %f\n",search_timeout); - if(search_timeout == 0){ - run_search = false; - } - } + }else if(inputstate == 6){ + search_timeout = atof(argv[i]); printf("search_timeout set to %f\n",search_timeout); + if(search_timeout == 0){ + run_search = false; + } + }else if(inputstate == 8){ + show_init_lvl = atoi(argv[i]); + }else if(inputstate == 9){ + show_refine_lvl = atoi(argv[i]); + }else if(inputstate == 10){ + show_reg_lvl = atoi(argv[i]); + } } if(visualization){ diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index da3c11ff..02b860c8 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -61,6 +61,10 @@ namespace reglib Model * model; Mesh * mesher; + int show_init_lvl;//init show + int show_refine_lvl;//refine show + bool show_scoring;//fuse scoring show + ModelUpdater(); ModelUpdater(Model * model_); ~ModelUpdater(); diff --git a/quasimodo/quasimodo_models/src/core/Util.cpp b/quasimodo/quasimodo_models/src/core/Util.cpp index 1838ef27..0311de6d 100644 --- a/quasimodo/quasimodo_models/src/core/Util.cpp +++ b/quasimodo/quasimodo_models/src/core/Util.cpp @@ -97,6 +97,7 @@ namespace reglib{ const double & nx = Xn(0,i); const double & ny = Xn(1,i); const double & nz = Xn(2,i); + const double & weight = W(i); double a = nz*sy - ny*sz; diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 3aece887..e451c285 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -156,9 +156,19 @@ ModelUpdater::ModelUpdater(){ occlusion_penalty = 10; massreg_timeout = 60; model = 0; + show_init_lvl = 0;//init show + show_refine_lvl = 0;//refine show + show_scoring = false;//fuse scoring show } -ModelUpdater::ModelUpdater(Model * model_){ model = model_;} +ModelUpdater::ModelUpdater(Model * model_){ + occlusion_penalty = 10; + massreg_timeout = 60; + show_init_lvl = 0;//init show + show_refine_lvl = 0;//refine show + show_scoring = false;//fuse scoring show + model = model_; +} ModelUpdater::~ModelUpdater(){} FusionResults ModelUpdater::registerModel(Model * model2, Eigen::Matrix4d guess, double uncertanity){return FusionResults();} @@ -323,7 +333,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M DistanceWeightFunction2 * func = new DistanceWeightFunction2(); func->f = THRESHOLD; - func->p = 0.03; + func->p = 0.05; Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} @@ -636,11 +646,15 @@ void ModelUpdater::makeInitialSetup(){ //// cv::waitKey(0); // } +// show_refine = false;//refine show +// show_reg = false;//registration show +// show_scoring = false;//fuse scoring sho MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); massreg->timeout = 4*massreg_timeout; massreg->viewer = viewer; - massreg->visualizationLvl = 0;//1; - massreg->maskstep = std::max(1,int(0.5+0.2*double(model->frames.size()))); + massreg->visualizationLvl = show_init_lvl; + + massreg->maskstep = std::max(1,int(0.5+0.3*double(model->frames.size()))); massreg->nomaskstep = std::max(5,int(0.5+1.0*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massreg->nomask = true; massreg->stopval = 0.0001; @@ -658,6 +672,20 @@ void ModelUpdater::makeInitialSetup(){ model->relativeposes.clear();// = mfr.poses; model->relativeposes.insert( model->relativeposes.end(), mfr.poses.begin()+model->submodels.size(), mfr.poses.end()); + vector > ocs = getOcclusionScores(model->relativeposes, model->frames, model->modelmasks, false, 1); + std::vector > scores = getScores(ocs); + std::vector partition = getPartition(scores,2,5,2); + + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + if(scores[i][j] >= 0){printf(" ");} + printf("%5.5f ",0.00001*scores[i][j]); + } + printf("\n"); + } + printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); + + model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); vector cp; @@ -673,6 +701,21 @@ void ModelUpdater::makeInitialSetup(){ model->rep_relativeposes = cp; model->rep_frames = cf; model->rep_modelmasks = cm; + +// vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); +// std::vector > scores2 = getScores(ocs2); + +// double sumscore_aft = 0; +// for(unsigned int i = 0; i < scores2.size(); i++){ +// for(unsigned int j = 0; j < scores2.size(); j++){ +// sumscore_aft += scores2[i][j]; +// } +// } + +// printf("bef %f after %f\n",sumscore_bef,sumscore_aft); +// if(sumscore_aft >= sumscore_bef){ +// model->submodels_relativeposes = mfr.poses; +// } } void ModelUpdater::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, int type, bool debugg){ @@ -1687,7 +1730,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * DistanceWeightFunction2 * func = new DistanceWeightFunction2(); func->f = THRESHOLD; - func->p = 0.03; + func->p = 0.05; Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} @@ -2169,7 +2212,7 @@ vector > ModelUpdater::getOcclusionScores(std::vector< scores.resize(occlusionScores.size()); for(unsigned int i = 0; i < occlusionScores.size(); i++){scores[i].resize(occlusionScores.size());} - bool lock = true; + bool lock = false; for(unsigned int i = 0; i < current_frames.size(); i++){ scores[i][i] = 0; for(unsigned int j = i+1; j < current_frames.size(); j++){ diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp index 0d10bfe1..a29fb6e8 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp @@ -14,11 +14,19 @@ namespace reglib ModelUpdaterBasicFuse::ModelUpdaterBasicFuse(Registration * registration_){ registration = registration_; model = new reglib::Model(); + + show_init_lvl = 0;//init show + show_refine_lvl = 0;//refine show + show_scoring = false;//fuse scoring show } ModelUpdaterBasicFuse::ModelUpdaterBasicFuse(Model * model_, Registration * registration_){ registration = registration_; model = model_; + + show_init_lvl = 0;//init show + show_refine_lvl = 0;//refine show + show_scoring = false;//fuse scoring show } ModelUpdaterBasicFuse::~ModelUpdaterBasicFuse(){} @@ -161,7 +169,7 @@ UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, int step = 0.5 + expectedCost/(10.0*11509168.5);// ~10 sec predicted max time step = std::max(1,step); - vector > ocs = computeOcclusionScore(models,rps,step,true); + vector > ocs = computeOcclusionScore(models,rps,step,show_scoring); std::vector > scores = getScores(ocs); std::vector partition = getPartition(scores,2,5,2); diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp index f9cfa5dd..c0c49426 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp @@ -210,11 +210,12 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ int tpbef = refinement->target_points; - - for(int tp = 500; tp <= 1000; tp *= 2){ + int mul = 1; + for(int tp = 500; tp <= 16000; tp *= 2){ + std::sort (fr_X.begin(), fr_X.end(), compareFusionResults); refinement->target_points = tp; - unsigned int nr_X = fr_X.size(); + unsigned int nr_X = fr_X.size()/mul; #pragma omp parallel for num_threads(8) for(unsigned int ax = 0; ax < nr_X; ax++){ fr_X[ax] = refinement->getTransform(fr_X[ax].guess); @@ -229,6 +230,7 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ } } } + mul *= 2; } // for(int tp = 500; tp <= 1000; tp *= 2){ @@ -260,13 +262,15 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ } -// refinement->visualizationLvl = 2; -// refinement->target_points = 1000; -// for(unsigned int ax = 0; ax < fr_X.size() && ax < 15; ax++){ -// printf("%i -> %f\n",ax,fr_X[ax].score); -// refinement->getTransform(fr_X[ax].guess); -// } -// refinement->visualizationLvl = 0; + if(visualizationLvl > 0){ + refinement->visualizationLvl = 2; + refinement->target_points = 10000; + for(unsigned int ax = 0; ax < fr_X.size() && ax < 5; ax++){ + printf("%i -> %f\n",ax,fr_X[ax].score); + refinement->getTransform(fr_X[ax].guess); + } + refinement->visualizationLvl = 0; + } refinement->target_points = tpbef; From 242bd6e85983fabbc0062ebf950a74309db07d0e Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 6 Jul 2016 13:22:46 +0200 Subject: [PATCH 030/255] added color to published clouds --- quasimodo/quasimodo_brain/src/modelserver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index f8993b4f..9a441569 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -114,7 +114,7 @@ void publishDatabasePCD(bool original_colors = false){ float maxx = 0; for(unsigned int i = 0; i < results.size(); i++){ - pcl::PointCloud::Ptr cloud = results[i]->getPCLcloud(1, original_colors); + pcl::PointCloud::Ptr cloud = results[i]->getPCLcloud(1, false); float meanx = 0; float meany = 0; float meanz = 0; @@ -159,7 +159,7 @@ void publish_history(std::vector::Ptr> history } void publish_places(reglib::Model * mod){ - pcl::PointCloud::Ptr cloud = mod->getPCLcloud(1,true); + pcl::PointCloud::Ptr cloud = mod->getPCLcloud(1,false); for(unsigned int i = 0; i < mod->submodels.size(); i++){ Eigen::Matrix4d pose = mod->submodels[i]->frames.front()->pose * mod->submodels_relativeposes[i].inverse(); pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); @@ -223,7 +223,7 @@ void show_sorted(){ viewer->removeAllPointClouds(); float maxx = 0; for(unsigned int i = 0; i < results.size(); i++){ - pcl::PointCloud::Ptr cloud = results[i]->getPCLcloud(1, false); + pcl::PointCloud::Ptr cloud = results[i]->getPCLcloud(1, false); float meanx = 0; float meany = 0; float meanz = 0; From 9453459204d4a7546573c911f65d7bc8b33645e7 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Sat, 9 Jul 2016 09:04:32 +0200 Subject: [PATCH 031/255] during traveling... --- quasimodo/quasimodo_brain/src/modelserver.cpp | 28 +- .../src/preload_object_data.cpp | 23 +- .../quasimodo_brain/src/robot_listener.cpp | 15 +- .../quasimodo_models/include/core/RGBDFrame.h | 2 +- .../quasimodo_models/include/core/Util.h | 51 +++ .../quasimodo_models/src/core/RGBDFrame.cpp | 6 +- quasimodo/quasimodo_models/src/core/Util.cpp | 1 + .../quasimodo_models/src/model/Model.cpp | 73 +++- .../src/modelupdater/ModelUpdater.cpp | 342 ++++++++++-------- .../registration/RegistrationRefinement.cpp | 25 ++ .../DistanceWeightFunction2PPR2.cpp | 2 +- 11 files changed, 397 insertions(+), 171 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 9a441569..784a407e 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -144,7 +144,7 @@ void publishDatabasePCD(bool original_colors = false){ sensor_msgs::PointCloud2 input; pcl::toROSMsg (*conccloud,input);//, *transformed_cloud); - input.header.frame_id = "/db_frame"; + input.header.frame_id = "/map"; database_pcd_pub.publish(input); } @@ -153,7 +153,7 @@ void publish_history(std::vector::Ptr> history for(unsigned int i = 0; i < history.size(); i++){ sensor_msgs::PointCloud2 input; pcl::toROSMsg (*history[i],input);//, *transformed_cloud); - input.header.frame_id = "/db_frame"; + input.header.frame_id = "/map"; model_history_pub.publish(input); } } @@ -182,6 +182,27 @@ void publishObject(int id){ } void dumpDatabase(std::string path = "."){ + + char command [1024]; + sprintf(command,"rm -r -f %s/database_tmp",path.c_str()); + printf("%s\n",command); + int r = system(command); + + sprintf(command,"mkdir %s/database_tmp",path.c_str()); + printf("%s\n",command); + r = system(command); + + for(unsigned int m = 0; m < modeldatabase->models.size(); m++){ + char buf [1024]; + sprintf(buf,"%s/database_tmp/model_%08i",path.c_str(),m); + sprintf(command,"mkdir -p %s",buf); + r = system(command); + + + modeldatabase->models[m]->save(std::string(buf)); + } + + /* char command [1024]; sprintf(command,"rm -r -f %s/quasimodo_model*",path.c_str()); printf("%s\n",command); @@ -203,6 +224,7 @@ void dumpDatabase(std::string path = "."){ modeldatabase->models[m]->save(std::string(buf)); } + */ } void retrievalCallback(const quasimodo_msgs::retrieval_query_result & qr){ @@ -799,6 +821,8 @@ delete reg; publish_history(modeldatabase->models[i]->getHistory()); }*/ show_sorted(); + publishDatabasePCD(); + dumpDatabase(savepath); //exit(0); diff --git a/quasimodo/quasimodo_brain/src/preload_object_data.cpp b/quasimodo/quasimodo_brain/src/preload_object_data.cpp index db32da70..abc0e789 100644 --- a/quasimodo/quasimodo_brain/src/preload_object_data.cpp +++ b/quasimodo/quasimodo_brain/src/preload_object_data.cpp @@ -141,7 +141,8 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg) { ROS_INFO("I heard: [%s]", msg->data.c_str()); - for(unsigned int i = 1; i < rgbs.size(); i++){ + for(unsigned int i = 0; i < rgbs.size(); i++){ + printf("feeding system...\n"); std::vector fid; std::vector fadded; for(unsigned int j = 0; j < rgbs[i].size(); j++){ @@ -473,7 +474,7 @@ void load(std::string sweep_xml){ cv::Mat fullmask; fullmask.create(480,640,CV_8UC1); - unsigned char * maskdata = (unsigned char *)fullmask.data; + unsigned char *take maskdata = (unsigned char *)fullmask.data; for(int j = 0; j < 480*640; j++){maskdata[j] = 0;} @@ -602,19 +603,19 @@ int main(int argc, char** argv){ } } - for(unsigned int i = 0; i < rgbs.size(); i++){ - int ind = rand()%rgbs.size(); - std::iter_swap(rgbs.begin()+i,rgbs.begin()+ind); - std::iter_swap(depths.begin()+i,depths.begin()+ind); - std::iter_swap(masks.begin()+i,masks.begin()+ind); - std::iter_swap(tfs.begin()+i,tfs.begin()+ind); - std::iter_swap(initposes.begin()+i,initposes.begin()+ind); - } +// for(unsigned int i = 0; i < rgbs.size(); i++){ +// int ind = rand()%rgbs.size(); +// std::iter_swap(rgbs.begin()+i,rgbs.begin()+ind); +// std::iter_swap(depths.begin()+i,depths.begin()+ind); +// std::iter_swap(masks.begin()+i,masks.begin()+ind); +// std::iter_swap(tfs.begin()+i,tfs.begin()+ind); +// std::iter_swap(initposes.begin()+i,initposes.begin()+ind); +// } model_from_frame_client = n.serviceClient("model_from_frame"); fuse_models_client = n.serviceClient( "fuse_models"); get_model_client = n.serviceClient( "get_model"); index_frame_client = n.serviceClient( "index_frame"); - +printf("ready to give data\n"); ros::spin(); } diff --git a/quasimodo/quasimodo_brain/src/robot_listener.cpp b/quasimodo/quasimodo_brain/src/robot_listener.cpp index f816598d..ac2ba99e 100644 --- a/quasimodo/quasimodo_brain/src/robot_listener.cpp +++ b/quasimodo/quasimodo_brain/src/robot_listener.cpp @@ -260,10 +260,15 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg) ifsrv.request.frame.rgb = *(rgbBridgeImage.toImageMsg()); ifsrv.request.frame.depth = *(depthBridgeImage.toImageMsg()); - ifsrv.request.frame.camera.K[0] = 536.458000;//cams[i][j].fx(); - ifsrv.request.frame.camera.K[4] = 537.422000;//cams[i][j].fy(); - ifsrv.request.frame.camera.K[2] = 314.458000;//cams[i][j].cx(); - ifsrv.request.frame.camera.K[5] = 242.038000;//cams[i][j].cy(); +// ifsrv.request.frame.camera.K[0] = 536.458000;//cams[i][j].fx(); +// ifsrv.request.frame.camera.K[4] = 537.422000;//cams[i][j].fy(); +// ifsrv.request.frame.camera.K[2] = 314.458000;//cams[i][j].cx(); +// ifsrv.request.frame.camera.K[5] = 242.038000;//cams[i][j].cy(); +//533.796,0,314.863,0,0,533.113,241.271,0,0,0,1,0, + ifsrv.request.frame.camera.K[0] = 533.796; + ifsrv.request.frame.camera.K[4] = 533.113; + ifsrv.request.frame.camera.K[2] = 314.863; + ifsrv.request.frame.camera.K[5] = 241.271; if (index_frame_client.call(ifsrv)){//Add frame to model server int frame_id = ifsrv.response.frame_id; fadded.push_back(j); @@ -472,7 +477,7 @@ void chatterCallback2(const std_msgs::String::ConstPtr& msg) } int main(int argc, char** argv){ - ros::init(argc, argv, "use_rares_client"); + ros::init(argc, argv, "robot_listener"); ros::NodeHandle n; std::string listentopic = "/some/stupid/topic"; diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 052091a0..44526b6d 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -50,7 +50,7 @@ namespace reglib void show(bool stop = false); pcl::PointCloud::Ptr getPCLcloud(); - void savePCD(std::string path = "cloud.pcd"); + void savePCD(std::string path = "cloud.pcd", Eigen::Matrix4d pose = Eigen::Matrix4d::Identity()); void save(std::string path = ""); static RGBDFrame * load(Camera * cam, std::string path); diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index e55ef7ad..20a80e12 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -33,6 +33,55 @@ using ceres::Solve; namespace reglib { + +/** + * @brief Returns homogenous 4x4 transformation matrix for given rotation (quaternion) and translation components + * @param q rotation represented as quaternion + * @param trans homogenous translation + * @return tf 4x4 homogeneous transformation matrix + * + */ +inline Eigen::Matrix4f +RotTrans2Mat4f(const Eigen::Quaternionf &q, const Eigen::Vector4f &trans) +{ + Eigen::Matrix4f tf = Eigen::Matrix4f::Identity();; + tf.block<3,3>(0,0) = q.toRotationMatrix(); + tf.block<4,1>(0,3) = trans; + tf(3,3) = 1.f; + return tf; +} + + +/** + * @brief Returns homogenous 4x4 transformation matrix for given rotation (quaternion) and translation components + * @param q rotation represented as quaternion + * @param trans translation + * @return tf 4x4 homogeneous transformation matrix + * + */ +inline Eigen::Matrix4f +RotTrans2Mat4f(const Eigen::Quaternionf &q, const Eigen::Vector3f &trans) +{ + Eigen::Matrix4f tf = Eigen::Matrix4f::Identity(); + tf.block<3,3>(0,0) = q.toRotationMatrix(); + tf.block<3,1>(0,3) = trans; + return tf; +} + +/** + * @brief Returns rotation (quaternion) and translation components from a homogenous 4x4 transformation matrix + * @param tf 4x4 homogeneous transformation matrix + * @param q rotation represented as quaternion + * @param trans homogenous translation + */ +inline void +Mat4f2RotTrans(const Eigen::Matrix4f &tf, Eigen::Quaternionf &q, Eigen::Vector4f &trans) +{ + Eigen::Matrix3f rotation = tf.block<3,3>(0,0); + q = rotation; + trans = tf.block<4,1>(0,3); +} + //namespace nanoflann { // /// KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage. // /// This code is adapted from the KDTreeEigenMatrixAdaptor class of nanoflann.hpp @@ -274,6 +323,8 @@ struct PointCloud template bool kdtree_get_bbox(BBOX& /* bb */) const { return false; } }; + + } typedef nanoflann::KDTreeSingleIndexAdaptor< nanoflann::L2_Simple_Adaptor > , reglib::ArrayData3D,3> Tree3d; diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 2ee5705c..86d82cd3 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -453,7 +453,8 @@ pcl::PointCloud::Ptr RGBDFrame::getPCLcloud(){ return cloud_ptr; } -void RGBDFrame::savePCD(std::string path){ +void RGBDFrame::savePCD(std::string path, Eigen::Matrix4d pose){ + printf("saving pcd: %s\n",path.c_str()); unsigned char * rgbdata = (unsigned char *)rgb.data; unsigned short * depthdata = (unsigned short *)depth.data; @@ -483,6 +484,9 @@ void RGBDFrame::savePCD(std::string path){ } } } + + //Mat4f2RotTrans(const Eigen::Matrix4f &tf, Eigen::Quaternionf &q, Eigen::Vector4f &trans) + Mat4f2RotTrans(pose.cast(),cloud->sensor_orientation_,cloud->sensor_origin_); int success = pcl::io::savePCDFileBinaryCompressed(path,*cloud); } diff --git a/quasimodo/quasimodo_models/src/core/Util.cpp b/quasimodo/quasimodo_models/src/core/Util.cpp index 0311de6d..eed25a70 100644 --- a/quasimodo/quasimodo_models/src/core/Util.cpp +++ b/quasimodo/quasimodo_models/src/core/Util.cpp @@ -1,6 +1,7 @@ #include "core/Util.h" namespace reglib{ + double getTime(){ struct timeval start1; gettimeofday(&start1, NULL); diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index e9d16b44..9388d110 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -1,5 +1,6 @@ #include "model/Model.h" #include +#include namespace reglib { @@ -721,7 +722,7 @@ pcl::PointCloud::Ptr Model::getPCLcloud(int step, bool color){ p.b = rgbdata[3*ind+0]; p.g = rgbdata[3*ind+1]; p.r = rgbdata[3*ind+2]; - }else{ + }else{#include p.b = pb; p.g = pg; p.r = pr; @@ -738,14 +739,15 @@ pcl::PointCloud::Ptr Model::getPCLcloud(int step, bool color){ } void Model::save(std::string path){ - unsigned long buffersize = (1+1+16*frames.size()+1+frames.size()*frames.size()+1*frames.size())*sizeof(double); + /* + unsigned long buffersize = (1+1+16*frames.size());//+1+frames.size()*frames.size()+1*frames.size())*sizeof(double); char* buffer = new char[buffersize]; double * buffer_double = (double *)buffer; unsigned long * buffer_long = (unsigned long *)buffer; int counter = 0; buffer_long[counter++] = frames.size(); - buffer_double[counter++] = score; + //buffer_double[counter++] = score; for(unsigned int f = 0; f < frames.size(); f++){ Eigen::Matrix4d pose = relativeposes[f]; for(int i = 0; i < 4; i++){ @@ -772,11 +774,73 @@ void Model::save(std::string path){ outfile.write (buffer,buffersize); outfile.close(); delete[] buffer; +*/ + char buf [1024]; + struct stat sb; + + if (stat(path.c_str(), &sb) == 0 && S_ISDIR(sb.st_mode)){ + sprintf(buf,"rm -r %s",path.c_str()); + system(buf); + } + sprintf(buf,"mkdir %s",path.c_str()); + system(buf); ofstream posesfile; - posesfile.open (path+"/poses.txt"); + posesfile.open (path+"/relativeposes.txt"); + for(unsigned int f = 0; f < frames.size(); f++){ + posesfile << relativeposes[f] << endl <save(std::string(buf)); + + sprintf(buf,"%s/frame_%08i.pcd",path.c_str(),f); + frames[f]->savePCD(std::string(buf),relativeposes[f]); + + sprintf(buf,"%s/modelmask_%08i.png",path.c_str(),f); + cv::imwrite( buf, modelmasks[f]->getMask() ); + } + posesfile.close(); + + + ofstream submodels_posesfile; + submodels_posesfile.open (path+"/submodels_relativeposes.txt"); + for(unsigned int i = 0; i < submodels.size(); i++){ + submodels_posesfile << submodels_relativeposes[i] << endl <save(std::string(buf)); + } + submodels_posesfile.close(); + /* + for(unsigned int f = 0; f < frames.size(); f++){ + char buf [1024]; + + sprintf(buf,"%s/views/cloud_%08i.pcd",path.c_str(),f); + frames[f]->savePCD(std::string(buf)); + + sprintf(buf,"%s/views/pose_%08i.txt",path.c_str(),f); + ofstream posefile; + posefile.open (buf); + + Eigen::Matrix4f eigen_tr(relativeposes[f].cast()); + Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, " ", " ", "", "", "", ""); + posefile << eigen_tr.format(CommaInitFmt)<maskvec; + sprintf(buf,"%s/views/object_indices_%08i.txt",path.c_str(),f); + ofstream indfile; + indfile.open (buf); + for(int i = 0; i < 640*480; i++){ + if(maskvec[i]){indfile << i << std::endl;} + } + indfile.close(); + //object_indices_00000030.txt + + } + */ +/* ofstream raresfile; raresfile.open (path+"/raresposes.txt"); Eigen::Matrix4f eigen_tr(Eigen::Matrix4f::Identity() ); @@ -828,6 +892,7 @@ void Model::save(std::string path){ //object_indices_00000030.txt } + */ } Model * Model::load(Camera * cam, std::string path){ diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index e451c285..d267e220 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -333,7 +333,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M DistanceWeightFunction2 * func = new DistanceWeightFunction2(); func->f = THRESHOLD; - func->p = 0.05; + func->p = 0.01; Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} @@ -432,7 +432,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M scloud->points[src_ind].g = 255.0*weight*weights.at(i); scloud->points[src_ind].b = 0; - if(i % 100 == 0){ + if(i % 300 == 0){ char buf [1024]; sprintf(buf,"line%i",i); viewer->addLine (scloud->points[src_ind], dcloud->points[dst_ind],buf); @@ -649,13 +649,29 @@ void ModelUpdater::makeInitialSetup(){ // show_refine = false;//refine show // show_reg = false;//registration show // show_scoring = false;//fuse scoring sho - MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); + + MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); + massreg->timeout = 4*massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = show_init_lvl; + + massreg->maskstep = std::max(1,int(0.25*double(model->frames.size()))); + massreg->nomaskstep = std::max(5,int(0.5+0.3*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg->nomask = true; + massreg->stopval = 0.0005; + + massreg->setData(model->frames,model->modelmasks); + MassFusionResults mfr = massreg->getTransforms(model->relativeposes); + + + /* + MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); massreg->timeout = 4*massreg_timeout; massreg->viewer = viewer; massreg->visualizationLvl = show_init_lvl; - massreg->maskstep = std::max(1,int(0.5+0.3*double(model->frames.size()))); - massreg->nomaskstep = std::max(5,int(0.5+1.0*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg->maskstep = std::max(1,int(0.25*double(model->frames.size()))); + massreg->nomaskstep = std::max(5,int(0.5+0.5*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massreg->nomask = true; massreg->stopval = 0.0001; @@ -667,7 +683,7 @@ void ModelUpdater::makeInitialSetup(){ //MassFusionResults mfr = massreg->getTransforms(model->relativeposes); MassFusionResults mfr = massreg->getTransforms(p); - +*/ model->relativeposes.clear();// = mfr.poses; model->relativeposes.insert( model->relativeposes.end(), mfr.poses.begin()+model->submodels.size(), mfr.poses.end()); @@ -687,7 +703,7 @@ void ModelUpdater::makeInitialSetup(){ model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); - +printf("getSuperPoints done\n"); vector cp; vector cf; vector cm; @@ -696,11 +712,12 @@ void ModelUpdater::makeInitialSetup(){ cf.push_back(model->frames[i]); cm.push_back(model->modelmasks[i]); } - getGoodCompareFrames(cp,cf,cm); + //getGoodCompareFrames(cp,cf,cm); model->rep_relativeposes = cp; model->rep_frames = cf; model->rep_modelmasks = cm; + model->save("latestModel"); // vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); // std::vector > scores2 = getScores(ocs2); @@ -1260,13 +1277,11 @@ void ModelUpdater::recomputeScores(){ } void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2){ -/* - unsigned char * src_maskdata = (unsigned char *)(mask->mask.data); - unsigned char * src_rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(frame->depth.data); - float * src_normalsdata = (float *)(frame->normals.data); + unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); + float * src_normalsdata = (float *)(frame1->normals.data); - Camera * src_camera = frame->camera; + Camera * src_camera = frame1->camera; const unsigned int src_width = src_camera->width; const unsigned int src_height = src_camera->height; const float src_idepth = src_camera->idepth_scale; @@ -1275,109 +1290,67 @@ void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weight const float src_ifx = 1.0/src_camera->fx; const float src_ify = 1.0/src_camera->fy; - std::vector test_x; - std::vector test_y; - std::vector test_z; - std::vector test_nx; - std::vector test_ny; - std::vector test_nz; + unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); + float * dst_normalsdata = (float *)(frame2->normals.data); - std::vector & testw = mask->testw; - std::vector & testh = mask->testh; - - for(unsigned int ind = 0; ind < testw.size();ind++){ - unsigned int src_w = testw[ind]; - unsigned int src_h = testh[ind]; - - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] == 255){ - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; + Camera * dst_camera = frame2->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; - test_x.push_back(src_x); - test_y.push_back(src_y); - test_z.push_back(src_z); - test_nx.push_back(src_nx); - test_ny.push_back(src_ny); - test_nz.push_back(src_nz); - } - } - } + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); bool debugg = false; - unsigned int nrdata_start = test_x.size(); - - for(unsigned int c = 0; c < cp.size();c++){ - RGBDFrame* dst = cf[c]; - if(dst == frame){continue;} - ModelMask* dst_modelmask = cm[c]; - unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); - unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); - float * dst_normalsdata = (float *)(dst->normals.data); - - Matrix4d rp = cp[c].inverse()*p; - - float m00 = rp(0,0); float m01 = rp(0,1); float m02 = rp(0,2); float m03 = rp(0,3); - float m10 = rp(1,0); float m11 = rp(1,1); float m12 = rp(1,2); float m13 = rp(1,3); - float m20 = rp(2,0); float m21 = rp(2,1); float m22 = rp(2,2); float m23 = rp(2,3); - - Camera * dst_camera = dst->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; -pcl::PointCloud::Ptr cloud (new pcl::PointCloud); -if(debugg){ - viewer->removeAllPointClouds(); - - pcl::PointCloud::Ptr cloud2 = dst->getPCLcloud(); - viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud2"); -} - - for(unsigned int ind = 0; ind < test_x.size();ind++){ - float src_nx = test_nx[ind]; - float src_ny = test_ny[ind]; - float src_nz = test_nz[ind]; - - float src_x = test_x[ind]; - float src_y = test_y[ind]; - float src_z = test_z[ind]; - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - pcl::PointXYZRGBNormal point; - if(debugg){ - point.x = tx; - point.y = ty; - point.z = tz; - point.r = 255; - point.g = 0; - point.b = 0; + pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); + if(debugg){ + + src_cloud->points.resize(src_width*src_height); + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + point.r = 255; + point.g = 0; + point.b = 0; + src_cloud->points[src_ind] = point; + } } + } - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - + dst_cloud->points.resize(dst_width*dst_height); + for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); float dst_nx = dst_normalsdata[3*dst_ind+0]; if(dst_z > 0 && dst_nx != 2){ @@ -1387,52 +1360,114 @@ if(debugg){ float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - double dst_noise = dst_z * dst_z; - double point_noise = src_z * src_z; + pcl::PointXYZRGBNormal point; + point.x = dst_x; + point.y = dst_y; + point.z = dst_z; + point.r = 255; + point.g = 0; + point.b = 0; + dst_cloud->points[dst_ind] = point; + } + } + } + } + + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; - double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); - d *= compare_mul; + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; +// pcl::PointXYZRGBNormal point; +// if(debugg){ +// point.x = tx; +// point.y = ty; +// point.z = tz; +// point.r = 255; +// point.g = 0; +// point.b = 0; +// } - if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE - //if(d < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE - test_nx[ind] = test_nx.back(); test_nx.pop_back(); - test_ny[ind] = test_ny.back(); test_ny.pop_back(); - test_nz[ind] = test_nz.back(); test_nz.pop_back(); - test_x[ind] = test_x.back(); test_x.pop_back(); - test_y[ind] = test_y.back(); test_y.pop_back(); - test_z[ind] = test_z.back(); test_z.pop_back(); - ind--; -if(debugg){ - point.r = 0; - point.g = 255; - point.b = 0; -} + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + double dst_noise = dst_z * dst_z; + double point_noise = src_z * src_z; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; + + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE + //if(d < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE +// test_nx[ind] = test_nx.back(); test_nx.pop_back(); +// test_ny[ind] = test_ny.back(); test_ny.pop_back(); +// test_nz[ind] = test_nz.back(); test_nz.pop_back(); +// test_x[ind] = test_x.back(); test_x.pop_back(); +// test_y[ind] = test_y.back(); test_y.pop_back(); +// test_z[ind] = test_z.back(); test_z.pop_back(); +// ind--; +// if(debugg){ +// point.r = 0; +// point.g = 255; +// point.b = 0; +// } + } } } } -if(debugg){ - cloud->points.push_back(point); -} } + } -if(debugg){ - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); + if(debugg){ viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "cloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "cloud"); viewer->spin(); + } } + +cv::Mat getImageFromArray(unsigned int width, unsigned int height, double * arr){ + double maxval = 0; + for(unsigned int i = 0; i < width*height; i++){ + maxval = std::max(maxval,arr[i]); } - return double(test_x.size())/double(nrdata_start); -*/ + cv::Mat m; + m.create(height,width,CV_8UC3); + unsigned char * data = m.data; + return m; } void ModelUpdater::computeOcclusionAreas(vector cp, vector cf, vector cm){ @@ -1457,8 +1492,8 @@ void ModelUpdater::computeOcclusionAreas(vector cp, vector overlaps[i] = new double[nr_pixels]; total[i] = new double[nr_pixels]; for(unsigned int j = 0; j < nr_pixels; j++){ - overlaps[i][j] = 1; - total[i][j] = 1; + overlaps[i][j] = 0; + total[i][j] = 0; } } @@ -1479,7 +1514,22 @@ void ModelUpdater::computeOcclusionAreas(vector cp, vector unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; wo[i] = new double[nr_pixels]; for(unsigned int j = 0; j < nr_pixels; j++){ - wo[i][j] = 1; + if(total[i][j] == 0){ + wo[i][j] = 0.0; + }else{ + wo[i][j] = overlaps[i][j]/total[i][j]; + } + } + + double sum = 0; + for(unsigned int j = 0; j < nr_pixels; j++){ + sum += wo[i][j]; + } + sum /= double(nr_pixels); + for(unsigned int j = 0; j < nr_pixels; j++){ + if(total[i][j] == 0){ + wo[i][j] = sum; + } } } weights_old.push_back(wo); @@ -1730,7 +1780,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * DistanceWeightFunction2 * func = new DistanceWeightFunction2(); func->f = THRESHOLD; - func->p = 0.05; + func->p = 0.01; Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp index 403c6ef6..76136515 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp @@ -284,6 +284,31 @@ bool timestopped = false; W = W.array()*rangeW.array()*rangeW.array(); + if(visualizationLvl >= 3){ + //printf("start show\n"); + unsigned int s_nr_data = X.cols(); + unsigned int d_nr_data = Y.cols(); + //printf("nr datas: %i %i\n",s_nr_data,d_nr_data); + + + viewer->removeAllPointClouds(); + pcl::PointCloud::Ptr scloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + + scloud->points.clear(); + dcloud->points.clear(); + for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);} + for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);} + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "dcloud"); + //printf("pre\n"); + viewer->spin(); + //printf("post\n"); + viewer->removeAllPointClouds(); + } + switch(type) { case PointToPoint: { pcl::TransformationFromCorrespondences tfc1; diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp index 950b7f98..41179ca6 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp @@ -651,7 +651,7 @@ bool DistanceWeightFunction2PPR2::update(){ double regularization_before = regularization; for(int i = 0; i < 500; i++){ iteration++; - regularization *= 0.5; + regularization *= 0.5; double change = histogram_size*regularization/maxd; //printf("DistanceWeightFunction2PPR2::update() %i -> reg %f change %f\n",i,regularization,change); From 05045983eae3d9fe588f716df01908ad73707168 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Sat, 9 Jul 2016 10:44:48 +0200 Subject: [PATCH 032/255] started testing segmenter --- quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index d267e220..06eb91a6 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -714,6 +714,9 @@ printf("getSuperPoints done\n"); } //getGoodCompareFrames(cp,cf,cm); + + computeOcclusionAreas(cp, cf,cm); + model->rep_relativeposes = cp; model->rep_frames = cf; model->rep_modelmasks = cm; From e43fdb1a803f48c3d124dee443a1fac8ea0a9a38 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Thu, 14 Jul 2016 11:42:58 +0200 Subject: [PATCH 033/255] Removed unnecessary printouts --- quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp index 7c9c7238..2f728c6e 100644 --- a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp @@ -241,14 +241,14 @@ pair get_offsets_in_data(const boost::filesystem::path& sweep_pa // skips 0, that's ok if (sweep_i != last_sweep) { // this should actually work, right? - cout << sweep_path.string() << endl; - cout << segment_path.parent_path().parent_path().string() << endl; + //cout << sweep_path.string() << endl; + //cout << segment_path.parent_path().parent_path().string() << endl; if (sweep_path == segment_path.parent_path().parent_path()) { cout << "Matching!" << endl; return make_pair(sweep_i, counter); } else { - cout << "Not matching!" << endl; + //cout << "Not matching!" << endl; } } From 48e06455e090893cb8310dff12fedc677da8990b Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Sat, 16 Jul 2016 23:03:30 +0200 Subject: [PATCH 034/255] Adding and retrieval somewhat working, also fixed a bug in retrieval_vocabulary --- .../dynamic_retrieval.h | 23 ++++++++--- .../scripts/retrieve_object_search.py | 8 +++- .../src/retrieval_vocabulary.cpp | 38 +++++++++++-------- 3 files changed, 46 insertions(+), 23 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index 38b9574f..e518eb93 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -62,15 +62,24 @@ std::vector get_retrieved_paths(const std::vector= offset) { + std::cout << "1. Index: " << s.index << " is larger than: " << offset << std::endl; std::cout << "URI loading does not support noise+annotated data structure!" << std::endl; + std::cout << "Got URI: " << uris.uris[s.index] << std::endl; exit(-1); } std::string uri = uris.uris[s.index]; - std::cout << "Got URI: " << uri << std::endl; + if (verbose) { + std::cout << "Got URI: " << uri << " at index: " << s.index << std::endl; + } if (uri.compare(0, 10, "mongodb://") == 0) { - std::string db_uri = uri.substr(7, uri.size()-7); + std::string db_uri = uri.substr(10, uri.size()-10); std::vector strs; boost::split(strs, db_uri, boost::is_any_of("/")); std::cout << "Initializing message story proxy with database: " << strs[0] << ", collection: " << strs[1] << std::endl; @@ -87,7 +96,9 @@ std::vector get_retrieved_paths(const std::vector= offset) { + std::cout << "2. Index: " << s.index << " is larger than: " << offset << std::endl; std::cout << "URI loading does not support noise+annotated data structure!" << std::endl; + std::cout << "Got URI: " << uris.uris[s.index] << std::endl; exit(-1); } std::string uri = uris.uris[s.index]; @@ -97,20 +108,20 @@ std::vector get_retrieved_paths(const std::vector strs; boost::split(strs, db_uri, boost::is_any_of("/")); // how do we get the home directory correctly? - boost::filesystem::path temp_path = boost::filesystem::path("~/.ros/quasimodo/temp") / (strs[2] + ".pcd"); + boost::filesystem::path temp_path = boost::filesystem::absolute(boost::filesystem::path("quasimodo/temp") / (strs[2] + ".pcd")); if (boost::filesystem::exists(temp_path)) { retrieved_paths.push_back(temp_path); continue; } - if (!boost::filesystem::exists("~/.ros/quasimodo/temp")) { - boost::filesystem::create_directories("~/.ros/quasimodo/temp"); + if (!boost::filesystem::exists("quasimodo/temp")) { + boost::filesystem::create_directories("quasimodo/temp"); } boost::shared_ptr message; diff --git a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py index d3824d36..b1742d72 100755 --- a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py +++ b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py @@ -26,6 +26,7 @@ import std_msgs pub = () +cloud_pub = () def retrieval_callback(object_id): @@ -89,16 +90,21 @@ def retrieval_callback(object_id): query.cloud = resp.processed_cloud query.depth = depths[0] query.image = images[0] - query.mask = masks[0] + cv_bgr_mask = cv_image = CvBridge().imgmsg_to_cv2(masks[0], "bgr8") + cv_mask = cv2.cvtColor(cv_bgr_mask, cv2.COLOR_BGR2GRAY) + query.mask = CvBridge().cv2_to_imgmsg(cv_mask, "mono8") #masks[0] query.number_query = 10 query.room_transform = transforms[0] pub.publish(query) + resp.processed_cloud.header.frame_id = "/map" + cloud_pub.publish(resp.processed_cloud) print("done") if __name__ == '__main__': rospy.init_node('retrieve_object_search', anonymous = False) pub = rospy.Publisher("/models/query", data_class=retrieval_query, queue_size=None) + cloud_pub = rospy.Publisher("/models/fused", data_class=PointCloud2, queue_size=None) sub = rospy.Subscriber("/models/mongodb_query", std_msgs.msg.String, callback=retrieval_callback) rospy.spin() diff --git a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp index bf54b654..0a9a0ef7 100644 --- a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp @@ -175,6 +175,9 @@ void train_vocabulary(const boost::filesystem::path& data_path) sweep_counter = 0; } + // here we insert in the uris + uris.uris.push_back(string("file://") + segment_path.string()); + if (features_i->size() < min_segment_features) { ++counter; ++sweep_counter; @@ -196,9 +199,6 @@ void train_vocabulary(const boost::filesystem::path& data_path) indices.push_back(index); } - // here we insert in the uris - uris.uris.push_back(string("file://") + segment_path.string()); - ++counter; ++sweep_counter; if (sweep_i >= min_training_sweeps && islast) { @@ -288,8 +288,12 @@ bool vocabulary_service(quasimodo_msgs::index_cloud::Request& req, quasimodo_msg // the question is what offsets we should assign these, I think in reality it doesn't matter, possibly the sweep offset might not get set automatically // let's just try 0, 0 for now //IndexT index(sweep_offset + sweep_i, offset, 0); // we only have one segment within these observations -> sweep_index = 0 - IndexT index(0, 0, 0); // we only have one segment within these observations -> sweep_index = 0 - indices.push_back(index); + dynamic_object_retrieval::vocabulary_summary summary; + summary.load(vocabulary_path); + IndexT index(100000, summary.nbr_noise_segments, 0); // this is a bit weird, did not think we relied on these, we only have one segment within these observations -> sweep_index = 0 + for (size_t i = 0; i < features->size(); ++i) { + indices.push_back(index); + } AdjacencyT adjacencies; if (features->size() > 0) { @@ -298,8 +302,8 @@ bool vocabulary_service(quasimodo_msgs::index_cloud::Request& req, quasimodo_msg { dynamic_object_retrieval::segment_uris uris; - if (boost::filesystem::exists(vocabulary_path / "segment_uris.json")) - { + if (boost::filesystem::exists(vocabulary_path / "segment_uris.json")) { + cout << "Segment uris file already exists, loading..." << endl; uris.load(vocabulary_path); } else { @@ -315,15 +319,12 @@ bool vocabulary_service(quasimodo_msgs::index_cloud::Request& req, quasimodo_msg } // This part is new! - { - dynamic_object_retrieval::vocabulary_summary summary; - summary.load(vocabulary_path); - resp.id = summary.nbr_noise_segments; - summary.nbr_noise_segments += 1; - summary.nbr_noise_sweeps++; - summary.save(vocabulary_path); - dynamic_object_retrieval::save_vocabulary(*vt, vocabulary_path); - } + + resp.id = summary.nbr_noise_segments; + summary.nbr_noise_segments += 1; + summary.nbr_noise_sweeps++; + summary.save(vocabulary_path); + dynamic_object_retrieval::save_vocabulary(*vt, vocabulary_path); return true; } @@ -348,6 +349,9 @@ void vocabulary_callback(const std_msgs::String::ConstPtr& msg) break; } ++nbr_segmented_sweeps; + if (boost::filesystem::path(xml) == sweep_xml) { + break; + } } if (nbr_segmented_sweeps >= min_training_sweeps + 1) { train_vocabulary(data_path); @@ -358,6 +362,8 @@ void vocabulary_callback(const std_msgs::String::ConstPtr& msg) return; } + cout << "After training: " << endl; + size_t offset; // same thing here, index among segments, can't take this from vocab, instead parse everything? size_t sweep_offset; // how do we get this to not require anything From ec53c125faf9f512fc9d058262425919ce2fb5fe Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Sat, 16 Jul 2016 23:56:15 +0200 Subject: [PATCH 035/255] Retrieval part now working, still small stuff left in viz --- .../dynamic_retrieval.h | 8 +-- .../src/quasimodo_retrieval_node.cpp | 56 +++++++++++++++---- 2 files changed, 50 insertions(+), 14 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index e518eb93..b3a417e5 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -113,15 +113,15 @@ std::vector get_retrieved_paths(const std::vector message; diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index 3b70eaec..6f4e4e39 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -216,6 +216,16 @@ class retrieval_node { // now probably do some dilation on the masks + if (get<0>(images).empty()) { + cv::Mat mask = cv::Mat::zeros(height, width, CV_8UC1); + cv::Mat depth = cv::Mat::zeros(height, width, CV_16UC1); + cv::Mat image = cv::Mat::zeros(height, width, CV_8UC3); + get<0>(images).push_back(mask); + get<1>(images).push_back(image); + get<2>(images).push_back(depth); + get<3>(images).push_back(-1); + } + return images; } @@ -272,6 +282,16 @@ class retrieval_node { } + if (get<0>(images).empty()) { + cv::Mat mask = cv::Mat::zeros(480, 640, CV_8UC1); + cv::Mat depth = cv::Mat::zeros(480, 640, CV_16UC1); + cv::Mat image = cv::Mat::zeros(480, 640, CV_8UC3); + get<0>(images).push_back(mask); + get<1>(images).push_back(image); + get<2>(images).push_back(depth); + get<3>(images).push_back(-1); + } + return images; } @@ -542,16 +562,12 @@ class retrieval_node { vector sweep_paths; //auto results = dynamic_object_retrieval::query_reweight_vocabulary(vt, refined_query, query_image, query_depth, // K, number_query, vocabulary_path, summary, false); - auto results = dynamic_object_retrieval::query_reweight_vocabulary((vocabulary_tree&)vt, features, 200, vocabulary_path, summary); + auto results = dynamic_object_retrieval::query_reweight_vocabulary((vocabulary_tree&)vt, features, 200, vocabulary_path, summary, true); // This is just to make sure that we have valid results even when some meta rooms have been deleted size_t counter = 0; while (counter < number_query) { // assume that everything in mongodb is kept - if (results.first[counter].first.stem().string() == "surfel_map") { - ++counter; - continue; - } if (!boost::filesystem::exists(results.first[counter].first)) { auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair::result_type>& v) { return boost::filesystem::exists(v.first); @@ -569,9 +585,24 @@ class retrieval_node { tie(retrieved_clouds, sweep_paths) = benchmark_retrieval::load_retrieved_clouds(results.first); - auto data = SimpleXMLParser::loadRoomFromXML(sweep_paths[0].string(), std::vector{"RoomIntermediateCloud"}, false, false); - tf::StampedTransform room_transform = data.vIntermediateRoomCloudTransforms[0]; - room_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0)); + cout << "Sweep paths:" << endl; + for (boost::filesystem::path sweep_path : sweep_paths) { + cout << sweep_path.string() << endl; + } + + + // FIXME: This is a hack that requires that at least one is not from MongoDB, needs fixing! + tf::StampedTransform room_transform; + for (int i = 0; i < retrieved_clouds.size(); ++i) { + string name = results.first[i].first.stem().string(); + cout << name << endl; + if (name != "surfel_map") { + auto data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); + room_transform = data.vIntermediateRoomCloudTransforms[0]; + room_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0)); + break; + } + } cout << "Query cloud size: " << cloud->size() << endl; for (CloudT::Ptr& c : retrieved_clouds) { @@ -583,6 +614,7 @@ class retrieval_node { for (auto s : results.first) { boost::filesystem::path segment_path = s.first; string name = segment_path.stem().string(); + cout << name << endl; if (name == "surfel_map") { // for the mongodb results indices.push_back(-1); } @@ -598,12 +630,12 @@ class retrieval_node { vector > masks(retrieved_clouds.size()); vector > images(retrieved_clouds.size()); vector > depths(retrieved_clouds.size()); - vector > paths(retrieved_clouds.size()); + vector > paths(retrieved_clouds.size()); for (int i = 0; i < retrieved_clouds.size(); ++i) { vector inds; vector > sweep_transforms; if (indices[i] == -1) { // special case for mongodb result - tie(masks[i], images[i], depths[i], inds) = generate_images_for_mongodb_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms); + tie(masks[i], images[i], depths[i], inds) = generate_images_for_mongodb_object(retrieved_clouds[i], K, boost::filesystem::path(results.first[i].first), sweep_transforms); } else { auto sweep_data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); @@ -670,8 +702,12 @@ class retrieval_node { pair sizes = make_pair(2, 5); + cout << "Images size: " << images.size() << endl; + cv::Mat visualization = cv::Mat::zeros(height*sizes.first, width*sizes.second, CV_8UC3); for (size_t i = 0; i < images.size(); ++i) { + cout << "Image " << i << " size: " << images[i].size() << endl; + cout << "Image " << i << " dimensions: " << images[i][0].rows << " x " << images[i][0].cols << endl; size_t offset_height = i / sizes.second; size_t offset_width = i % sizes.second; images[i][0].copyTo(visualization(cv::Rect(offset_width*width, offset_height*height, width, height))); From dbbf672a74e05b5f1abdcb2d2f88f38188ea08d3 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Sun, 17 Jul 2016 09:24:15 +0200 Subject: [PATCH 036/255] working on segmentation --- quasimodo/quasimodo_brain/CMakeLists.txt | 18 +- quasimodo/quasimodo_brain/src/Util/Util.cpp | 100 ++ quasimodo/quasimodo_brain/src/Util/Util.h | 43 + quasimodo/quasimodo_brain/src/modelserver.cpp | 5 - .../src/segmentationserver.cpp | 321 +++++ .../quasimodo_brain/src/test_segment.cpp | 198 ++- .../include/modelupdater/ModelUpdater.h | 3 +- .../registration/MassRegistrationPPR2.h | 1 + .../quasimodo_models/src/core/RGBDFrame.cpp | 18 + .../src/modelupdater/ModelUpdater.cpp | 1209 ++++++++++++++++- .../src/modelupdater/graphcuts/CHANGES.TXT | 29 + .../src/modelupdater/graphcuts/README.TXT | 123 ++ .../src/modelupdater/graphcuts/block.h | 268 ++++ .../src/modelupdater/graphcuts/graph.cpp | 115 ++ .../src/modelupdater/graphcuts/graph.h | 506 +++++++ .../src/modelupdater/graphcuts/instances.inc | 16 + .../src/modelupdater/graphcuts/maxflow.cpp | 684 ++++++++++ .../src/registration/MassRegistrationPPR2.cpp | 203 +++ .../DistanceWeightFunction2PPR2.cpp | 115 +- quasimodo/quasimodo_msgs/CMakeLists.txt | 1 + .../quasimodo_msgs/srv/segment_model.srv | 8 + 21 files changed, 3892 insertions(+), 92 deletions(-) create mode 100644 quasimodo/quasimodo_brain/src/Util/Util.cpp create mode 100644 quasimodo/quasimodo_brain/src/Util/Util.h create mode 100644 quasimodo/quasimodo_brain/src/segmentationserver.cpp create mode 100644 quasimodo/quasimodo_models/src/modelupdater/graphcuts/CHANGES.TXT create mode 100644 quasimodo/quasimodo_models/src/modelupdater/graphcuts/README.TXT create mode 100644 quasimodo/quasimodo_models/src/modelupdater/graphcuts/block.h create mode 100644 quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.cpp create mode 100644 quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.h create mode 100644 quasimodo/quasimodo_models/src/modelupdater/graphcuts/instances.inc create mode 100644 quasimodo/quasimodo_models/src/modelupdater/graphcuts/maxflow.cpp create mode 100644 quasimodo/quasimodo_msgs/srv/segment_model.srv diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 0a6527da..421aa503 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -85,7 +85,17 @@ catkin_package( include_directories(${catkin_INCLUDE_DIRS}) -add_library(quasimodo_ModelDatabase src/ModelDatabase/ModelDatabase.cpp src/ModelDatabase/ModelDatabaseBasic.cpp src/ModelDatabase/ModelDatabaseRGBHistogram.cpp)# src/ModelDatabase/ModelDatabaseRetrieval.cpp) + +add_library(quasimodo_brain_util src/Util/Util.cpp) +target_link_libraries(quasimodo_brain_util + #${retrieval_libraries} + #${benchmark_libraries} + ${OpenCV_LIBS} + ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} + ${PCL_LIBRARIES} + ${catkin_LIBRARIES}) + +add_library(quasimodo_ModelDatabase src/ModelDatabase/ModelDatabase.cpp src/ModelDatabase/ModelDatabaseBasic.cpp src/ModelDatabase/ModelDatabaseRGBHistogram.cpp) target_link_libraries(quasimodo_ModelDatabase #${retrieval_libraries} #${benchmark_libraries} @@ -132,7 +142,11 @@ target_link_libraries( test_align2 quasimodo_ModelDatabase quasimodo_Mesher ima add_executable( test_segment src/test_segment.cpp) add_dependencies( test_segment roscpp quasimodo_msgs_generate_messages_cpp) -target_link_libraries( test_segment quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries( test_segment quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + +add_executable( segmentationserver src/segmentationserver.cpp) +add_dependencies( segmentationserver roscpp quasimodo_msgs_generate_messages_cpp) +target_link_libraries( segmentationserver quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) ############# ## Install ## diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp new file mode 100644 index 00000000..4be36ecb --- /dev/null +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -0,0 +1,100 @@ +#include "Util.h" + +namespace quasimodo_brain { + +double getTime(){ + struct timeval start1; + gettimeofday(&start1, NULL); + return double(start1.tv_sec+(start1.tv_usec/1000000.0)); +} + +reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg){ + reglib::Model * model = new reglib::Model(); + + for(unsigned int i = 0; i < msg.local_poses.size(); i++){ + sensor_msgs::CameraInfo camera = msg.frames[i].camera; + ros::Time capture_time = msg.frames[i].capture_time; + geometry_msgs::Pose pose = msg.frames[i].pose; + + cv_bridge::CvImagePtr rgb_ptr; + try{ rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat rgb = rgb_ptr->image; + + cv_bridge::CvImagePtr depth_ptr; + try{ depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat depth = depth_ptr->image; + + Eigen::Affine3d epose; + tf::poseMsgToEigen(pose, epose); + + reglib::Camera * cam = new reglib::Camera(); + if(camera.K[0] > 0){ + cam->fx = camera.K[0]; + cam->fy = camera.K[4]; + cam->cx = camera.K[2]; + cam->cy = camera.K[5]; + } + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); + model->frames.push_back(frame); + + geometry_msgs::Pose pose1 = msg.local_poses[i]; + Eigen::Affine3d epose1; + tf::poseMsgToEigen(pose1, epose1); + model->relativeposes.push_back(epose1.matrix()); + + cv_bridge::CvImagePtr mask_ptr; + try{ mask_ptr = cv_bridge::toCvCopy(msg.masks[i], sensor_msgs::image_encodings::MONO8);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat mask = mask_ptr->image; + + model->modelmasks.push_back(new reglib::ModelMask(mask)); + } + model->recomputeModelPoints(); + return model; +} + +void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp){ + int startsize = msg.local_poses.size(); + msg.local_poses.resize(startsize+model->relativeposes.size()); + msg.frames.resize(startsize+model->frames.size()); + msg.masks.resize(startsize+model->modelmasks.size()); + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + geometry_msgs::Pose pose1; + tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i])*rp, pose1); + geometry_msgs::Pose pose2; + tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose)*rp, pose2); + cv_bridge::CvImage rgbBridgeImage; + rgbBridgeImage.image = model->frames[i]->rgb; + rgbBridgeImage.encoding = "bgr8"; + cv_bridge::CvImage depthBridgeImage; + depthBridgeImage.image = model->frames[i]->depth; + depthBridgeImage.encoding = "mono16"; + cv_bridge::CvImage maskBridgeImage; + maskBridgeImage.image = model->modelmasks[i]->getMask(); + maskBridgeImage.encoding = "mono8"; + msg.local_poses[startsize+i] = pose1; + msg.frames[startsize+i].capture_time = ros::Time(); + msg.frames[startsize+i].pose = pose2; + msg.frames[startsize+i].frame_id = model->frames[i]->id; + msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); + msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); + msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg());//getMask() + } + for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ + addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp); + } +} + +quasimodo_msgs::model getModelMSG(reglib::Model * model){ + quasimodo_msgs::model msg; + msg.model_id = model->id; + addToModelMSG(msg,model); + + + return msg; +} + +} diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h new file mode 100644 index 00000000..c5739441 --- /dev/null +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -0,0 +1,43 @@ +#ifndef brainUtil_H +#define brainUtil_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ros/ros.h" +#include "std_msgs/String.h" +#include +#include + +#include +#include + +#include "eigen_conversions/eigen_msg.h" +#include "tf_conversions/tf_eigen.h" + + +#include "quasimodo_msgs/segment_model.h" + +#include +#include + +#include "modelupdater/ModelUpdater.h" +#include "core/RGBDFrame.h" + +namespace quasimodo_brain { + +double getTime(); +reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg); + +void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp = Eigen::Affine3d::Identity()); +quasimodo_msgs::model getModelMSG(reglib::Model * model); + +} + +#endif diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 784a407e..cd4361ae 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -165,16 +165,11 @@ void publish_places(reglib::Model * mod){ pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); pcl::transformPointCloud (*cloud, *transformed_cloud, pose); - sensor_msgs::PointCloud2 input; pcl::toROSMsg (*transformed_cloud,input);//, *transformed_cloud); input.header.frame_id = "/map"; model_places_pub.publish(input); } -/* - for(unsigned int i = 0; i < history.size(); i++){ - } - */ } void publishObject(int id){ diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp new file mode 100644 index 00000000..134757f4 --- /dev/null +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -0,0 +1,321 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include +#include + +#include +#include + +#include "eigen_conversions/eigen_msg.h" +#include "tf_conversions/tf_eigen.h" + +#include "quasimodo_msgs/segment_model.h" + + +#include +#include + +#include "modelupdater/ModelUpdater.h" +#include "core/RGBDFrame.h" + +#include "Util/Util.h" + + +using namespace std; + + +bool visualization = false; +boost::shared_ptr viewer; + + +//Strategy: +//Register frames to background+other frames in the same model DONE +//Determine occlusions inside current model to filter out motion(most likeley people etc) //Done + //Remove from current model +//Determine occlusion of current model to background + //These pixels are dynamic objects +//Update background + //How? + +bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs::segment_model::Response & res){ + printf("segment_model\n"); + reglib::Model * bg = quasimodo_brain::getModelFromMSG(req.backgroundmodel); + std::vector< reglib::Model * > models; + for(unsigned int i = 0; i < req.models.size(); i++){ + models.push_back(quasimodo_brain::getModelFromMSG(req.models[i])); + } + + vector cp; + vector cf; + vector mm; + + vector cp_front; + vector cf_front; + vector mm_front; + for(int j = 0; j < models.size(); j++){ + cp_front.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); + cf_front.push_back(models[j]->frames.front()); + mm_front.push_back(models[j]->modelmasks.front()); + } + printf("cp_front: %i\n",cp_front.size()); + + if(models.size() > 2){ + reglib::MassRegistrationPPR2 * massreg = new reglib::MassRegistrationPPR2(0.05); + massreg->timeout = 1200; + massreg->viewer = viewer; + massreg->visualizationLvl = 0; + + massreg->maskstep = 5;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massreg->nomaskstep = 5;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg->nomask = true; + massreg->stopval = 0.0005; + + massreg->setData(cf_front,mm_front); + + + reglib::MassFusionResults mfr_front = massreg->getTransforms(cp_front); + + for(int j = models.size()-1; j >= 0; j--){ + Eigen::Matrix4d change = mfr_front.poses[j] * cp_front[j].inverse(); + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); + cf.push_back(models[j]->frames[k]); + mm.push_back(models[j]->modelmasks[k]); + } + } + }else{ + for(int j = models.size()-1; j >= 0; j--){ + Eigen::Matrix4d change = Eigen::Matrix4d::Identity();//mfr_front.poses[j] * cp_front[j].inverse(); + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); + cf.push_back(models[j]->frames[k]); + mm.push_back(models[j]->modelmasks[k]); + } + } + } + + + printf("cp: %i\n",cp.size()); + + reglib::MassRegistrationPPR2 * massreg2 = new reglib::MassRegistrationPPR2(0.0); + massreg2->timeout = 1200; + massreg2->viewer = viewer; + massreg2->visualizationLvl = 1; + + massreg2->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massreg2->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg2->nomask = true; + massreg2->stopval = 0.0005; + + massreg2->setData(cf,mm); + reglib::MassFusionResults mfr2 = massreg2->getTransforms(cp); + cp = mfr2.poses; + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + vector masks; + for(unsigned int i = 0; i < cf.size(); i++){ + cv::Mat mask; + mask.create(cf[i]->camera->height,cf[i]->camera->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int i = 0; i < cf[i]->camera->height*cf[i]->camera->width;i++){maskdata[i] = 255;} + masks.push_back(mask); + } + std::vector internal_masks = mu->computeDynamicObject(0,cp,cf,masks);//Determine self occlusions + std::vector dynamic_masks = mu->computeDynamicObject(bg,cp,cf,internal_masks);//Determine occlusion of background occlusions + //add new frames to background + //Compute minimum required frames to capture background + for(unsigned int i = 0; i < cf.size(); i++){ + + cv::Mat mask; + mask.create(cf[i]->camera->height,cf[i]->camera->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); + unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[i].data); + for(unsigned int i = 0; i < cf[i]->camera->height*cf[i]->camera->width;i++){ + maskdata[i] = std::max(internalmaskdata[i],dynamicmaskdata[i]); + } + + bg->frames.push_back(cf[i]); + bg->relativeposes.push_back(cp[i]); + bg->modelmasks.push_back(new reglib::ModelMask(mask)); + } + + return true; +} + +int main(int argc, char** argv){ + ros::init(argc, argv, "segmentationserver"); + ros::NodeHandle n; + + int inputstate = -1; + for(int i = 1; i < argc;i++){ + printf("input: %s\n",argv[i]); + if(std::string(argv[i]).compare("-v") == 0){ printf("visualization turned on\n"); visualization = true;} + /* + if( std::string(argv[i]).compare("-c") == 0){ printf("camera input state\n"); inputstate = 1;} + else if(std::string(argv[i]).compare("-l") == 0){ printf("reading all models at %s (the path defined from -p)\n",savepath.c_str()); + std::vector folderdata; + int r = getdir(savepath+"/",folderdata); + for(unsigned int fnr = 0; fnr < folderdata.size(); fnr++){ + printf("%s\n",folderdata[fnr].c_str()); + } + exit(0);} + else if(std::string(argv[i]).compare("-m") == 0){ printf("model input state\n"); inputstate = 2;} + else if(std::string(argv[i]).compare("-p") == 0){ printf("path input state\n"); inputstate = 3;} + else if(std::string(argv[i]).compare("-occlusion_penalty") == 0){printf("occlusion_penalty input state\n");inputstate = 4;} + else if(std::string(argv[i]).compare("-massreg_timeout") == 0){printf("massreg_timeout input state\n");inputstate = 5;} + else if(std::string(argv[i]).compare("-search") == 0){printf("pointcloud search input state\n");run_search = true; inputstate = 6;} + else + else if(std::string(argv[i]).compare("-v_init") == 0){ printf("visualization of init turned on\n"); visualization = true; inputstate = 8;} + else if(std::string(argv[i]).compare("-v_refine") == 0 || std::string(argv[i]).compare("-v_ref") == 0){ printf("visualization refinement turned on\n"); visualization = true; inputstate = 9;} + else if(std::string(argv[i]).compare("-v_register") == 0 || std::string(argv[i]).compare("-v_reg") == 0){ printf("visualization registration turned on\n"); visualization = true; inputstate = 10;} + else if(std::string(argv[i]).compare("-v_scoring") == 0 || std::string(argv[i]).compare("-v_score") == 0 || std::string(argv[i]).compare("-v_sco") == 0){ printf("visualization scoring turned on\n"); visualization = true; show_scoring = true;} + else if(std::string(argv[i]).compare("-v_db") == 0){ printf("visualization db turned on\n"); visualization = true; show_db = true;} + else if(inputstate == 1){ + reglib::Camera * cam = reglib::Camera::load(std::string(argv[i])); + delete cameras[0]; + cameras[0] = cam; + }else if(inputstate == 2){ + reglib::Model * model = reglib::Model::load(cameras[0],std::string(argv[i])); + sweepid_counter = std::max(int(model->modelmasks[0]->sweepid + 1), sweepid_counter); + modeldatabase->add(model); + //addToDB(modeldatabase, model,false); + model->last_changed = ++current_model_update; + show_sorted(); + }else if(inputstate == 3){ + savepath = std::string(argv[i]); + }else if(inputstate == 4){ + occlusion_penalty = atof(argv[i]); printf("occlusion_penalty set to %f\n",occlusion_penalty); + }else if(inputstate == 5){ + massreg_timeout = atof(argv[i]); printf("massreg_timeout set to %f\n",massreg_timeout); + }else if(inputstate == 6){ + search_timeout = atof(argv[i]); printf("search_timeout set to %f\n",search_timeout); + if(search_timeout == 0){ + run_search = false; + } + }else if(inputstate == 8){ + show_init_lvl = atoi(argv[i]); + }else if(inputstate == 9){ + show_refine_lvl = atoi(argv[i]); + }else if(inputstate == 10){ + show_reg_lvl = atoi(argv[i]); + } + */ + } + + if(visualization){ + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); + viewer->addCoordinateSystem(0.01); + viewer->setBackgroundColor(0.0,0.0,0.0); + } + + + ros::ServiceServer service = n.advertiseService("segment_model", segment_model); + ROS_INFO("Ready to add use segment_model."); + + ros::spin(); + +/* +exit(0); + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + ros::NodeHandle pn("~"); + for(int ar = 1; ar < argc; ar++){ + string overall_folder = std::string(argv[ar]); + + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + printf("sweep_xmls\n"); + for (auto sweep_xml : sweep_xmls) { + printf("sweep_xml: %s\n",sweep_xml.c_str()); + load2(sweep_xml); + } + } + + for(unsigned int i = 0; i < models.size(); i++){ + printf("%i -> %i\n",i,models[i]->frames.size()); + + vector cp; + vector cf; + vector mm; + + vector cp_front; + vector cf_front; + vector mm_front; + for(int j = 0; j <= i; j++){ + cp_front.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); + cf_front.push_back(models[j]->frames.front()); + mm_front.push_back(models[j]->modelmasks.front()); + } + + if(i > 0){ + reglib::MassRegistrationPPR2 * massreg = new reglib::MassRegistrationPPR2(0.05); + massreg->timeout = 1200; + massreg->viewer = viewer; + massreg->visualizationLvl = 0; + + massreg->maskstep = 5;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massreg->nomaskstep = 5;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg->nomask = true; + massreg->stopval = 0.0005; + + massreg->setData(cf_front,mm_front); + + + reglib::MassFusionResults mfr_front = massreg->getTransforms(cp_front); + + for(int j = i; j >= 0; j--){ + Eigen::Matrix4d change = mfr_front.poses[j] * cp_front[j].inverse(); + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); + cf.push_back(models[j]->frames[k]); + mm.push_back(models[j]->modelmasks[k]); + } + } + }else{ + for(int j = i; j >= 0; j--){ + Eigen::Matrix4d change = Eigen::Matrix4d::Identity();//mfr_front.poses[j] * cp_front[j].inverse(); + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); + cf.push_back(models[j]->frames[k]); + mm.push_back(models[j]->modelmasks[k]); + } + } + } + + reglib::MassRegistrationPPR2 * massreg2 = new reglib::MassRegistrationPPR2(0.0); + massreg2->timeout = 1200; + massreg2->viewer = viewer; + massreg2->visualizationLvl = 1; + + massreg2->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massreg2->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg2->nomask = true; + massreg2->stopval = 0.0005; + + massreg2->setData(cf,mm); + reglib::MassFusionResults mfr2 = massreg2->getTransforms(cp); + cp = mfr2.poses; + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models[i], reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + vector mats; + mu->computeDynamicObject(cp,cf,mats); + + //delete mu; + } + + + */ +} diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index 76d0dcef..cd129cee 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -31,6 +31,7 @@ #include "quasimodo_msgs/index_frame.h" #include "quasimodo_msgs/fuse_models.h" #include "quasimodo_msgs/get_model.h" +#include "quasimodo_msgs/segment_model.h" #include "ros/ros.h" #include @@ -57,6 +58,8 @@ #include "modelupdater/ModelUpdater.h" #include "core/RGBDFrame.h" +#include "Util/Util.h" + using namespace std; @@ -326,15 +329,57 @@ reglib::Model * load2(std::string sweep_xml){ SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS - cam->fx = 536.458000; - cam->fy = 537.422000; - cam->cx = 314.458000; - cam->cy = 242.038000; +// cam->fx = 536.458000; +// cam->fy = 537.422000; +// cam->cx = 314.458000; +// cam->cy = 242.038000; + + cam->fx = 532.158936; + cam->fy = 533.819214; + cam->cx = 310.514310; + cam->cy = 236.842039; + +/* + projection + 532.158936 0.000000 310.514310 0.000000 + 0.000000 533.819214 236.842039 0.000000 + 0.000000 0.000000 1.000000 0.000000 +*/ + +/* + [image] + + width + 640 + + height + 480 + + [narrow_stereo] + + camera matrix + 534.192642 0.000000 311.485658 + 0.000000 533.870030 237.668175 + 0.000000 0.000000 1.000000 + + distortion + 0.030388 -0.100645 -0.000995 -0.000366 0.000000 + + rectification + 1.000000 0.000000 0.000000 + 0.000000 1.000000 0.000000 + 0.000000 0.000000 1.000000 + + projection + 532.158936 0.000000 310.514310 0.000000 + 0.000000 533.819214 236.842039 0.000000 + 0.000000 0.000000 1.000000 0.000000 +*/ cv::Mat fullmask; fullmask.create(480,640,CV_8UC1); unsigned char * maskdata = (unsigned char *)fullmask.data; - for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} + for(int j = 0; j < 480*640; j++){maskdata[j] = 0;} reglib::Model * sweepmodel = 0; std::vector current_room_frames; @@ -396,11 +441,12 @@ reglib::Model * load2(std::string sweep_xml){ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); viewer->spin(); */ - pcl::PointCloud::Ptr cn = sweepmodel->getPCLnormalcloud(1, false); - viewer->removeAllPointClouds(); - viewer->addPointCloud (cn, pcl::visualization::PointCloudColorHandlerRGBField(cn), "sample cloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); - viewer->spin(); + +// pcl::PointCloud::Ptr cn = sweepmodel->getPCLnormalcloud(1, false); +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cn, pcl::visualization::PointCloudColorHandlerRGBField(cn), "sample cloud"); +// viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); +// viewer->spin(); return sweepmodel; } @@ -640,24 +686,148 @@ void load(std::string sweep_xml){ int main(int argc, char** argv){ - ros::init(argc, argv, "use_rares_client"); + ros::init(argc, argv, "test_segment"); ros::NodeHandle n; - ros::Subscriber sub = n.subscribe("modelserver", 1000, chatterCallback); +// ros::Subscriber sub = n.subscribe("modelserver", 1000, chatterCallback); viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0.5, 0, 0.5); viewer->addCoordinateSystem (1.0); viewer->initCameraParameters (); - ros::NodeHandle pn("~"); + ros::ServiceClient segmentation_client = n.serviceClient("segment_model"); + for(int ar = 1; ar < argc; ar++){ string overall_folder = std::string(argv[ar]); - vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + printf("sweep_xmls\n"); for (auto sweep_xml : sweep_xmls) { + printf("sweep_xml: %s\n",sweep_xml.c_str()); load2(sweep_xml); } } + for(unsigned int i = 0; i < models.size(); i++){ + quasimodo_msgs::segment_model sm; + sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); + if (segmentation_client.call(sm)){//Build model from frame + //int model_id = mff.response.model_id; + printf("segmented: %i\n",i); + }else{ROS_ERROR("Failed to call service segment_model");} + } + + +// mff.request.mask = *(maskBridgeImage.toImageMsg()); +// mff.request.isnewmodel = (j == (fadded.size()-1)); +// mff.request.frame_id = fid[j]; + +// if (model_from_frame_client.call(mff)){//Build model from frame +// int model_id = mff.response.model_id; +// if(model_id > 0){ +// ROS_INFO("model_id%i", model_id ); +// } +// }else{ROS_ERROR("Failed to call service index_frame");} +exit(0); +/* + ros::NodeHandle pn("~"); + + + for(unsigned int i = 0; i < models.size(); i++){ + printf("%i -> %i\n",i,models[i]->frames.size()); + + vector cp; + vector cf; + vector mm; + + vector cp_front; + vector cf_front; + vector mm_front; + for(int j = 0; j <= i; j++){ + cp_front.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); + cf_front.push_back(models[j]->frames.front()); + mm_front.push_back(models[j]->modelmasks.front()); + } + + if(i > 0){ + reglib::MassRegistrationPPR2 * massreg = new reglib::MassRegistrationPPR2(0.05); + massreg->timeout = 1200; + massreg->viewer = viewer; + massreg->visualizationLvl = 0; + + massreg->maskstep = 5;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massreg->nomaskstep = 5;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg->nomask = true; + massreg->stopval = 0.0005; + + // massreg->setData(models[i]->frames,models[i]->modelmasks); + // reglib::MassFusionResults mfr = massreg->getTransforms(models[i]->relativeposes); + + massreg->setData(cf_front,mm_front); + + + reglib::MassFusionResults mfr_front = massreg->getTransforms(cp_front); + + for(int j = i; j >= 0; j--){ + Eigen::Matrix4d change = mfr_front.poses[j] * cp_front[j].inverse(); + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); + cf.push_back(models[j]->frames[k]); + mm.push_back(models[j]->modelmasks[k]); + } + } + }else{ + for(int j = i; j >= 0; j--){ + Eigen::Matrix4d change = Eigen::Matrix4d::Identity();//mfr_front.poses[j] * cp_front[j].inverse(); + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); + cf.push_back(models[j]->frames[k]); + mm.push_back(models[j]->modelmasks[k]); + } + } + } + + + + + reglib::MassRegistrationPPR2 * massreg2 = new reglib::MassRegistrationPPR2(0.0); + massreg2->timeout = 1200; + massreg2->viewer = viewer; + massreg2->visualizationLvl = 1; + + massreg2->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massreg2->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg2->nomask = true; + massreg2->stopval = 0.0005; + + massreg2->setData(cf,mm); + reglib::MassFusionResults mfr2 = massreg2->getTransforms(cp); + cp = mfr2.poses; + + //massreg->setData(models[i]->frames,models[i]->modelmasks); + //reglib::MassFusionResults mfr = massreg->getTransforms(models[i]->relativeposes); + +// massreg->setData(cf,mm); +// reglib::MassFusionResults mfr = massreg->getTransforms(cp); +// cp = mfr.poses; + + + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models[i], reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + vector mats; + mu->computeDynamicObject(cp,cf,mats);//models[i]->relativeposes, models[i]->frames, mats ); + +// models[i]->print(); +// mu->show_init_lvl = 2; +// mu->makeInitialSetup(); +// models[i]->print(); + + //delete mu; + } +*/ ros::spin(); } diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 02b860c8..374eb740 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -90,7 +90,8 @@ namespace reglib virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); - + virtual void getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask); + virtual std::vector computeDynamicObject(reglib::Model * bg, vector cp, vector cf, vector masks ); virtual OcclusionScore computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1, bool debugg = false); virtual OcclusionScore computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp = Matrix4d::Identity(), int step = 1, bool debugg = false); diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h index fa0c69ed..1e4035af 100644 --- a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h +++ b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h @@ -139,6 +139,7 @@ namespace reglib ~MassRegistrationPPR2(); void clearData(); + void addModel(Model * model); void addModelData(Model * model, bool submodels = true); void addData(RGBDFrame* frame, ModelMask * mmask); void addData(pcl::PointCloud::Ptr cloud); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 86d82cd3..22483f88 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -242,6 +242,24 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } +// cv::Mat curvature; +// curvature.create(height,width,CV_8UC3); +// unsigned char * curvaturedata = (unsigned char *)curvature.data; +// for(int w = 0; w < width; w++){ +// for(int h = 0; h < height;h++){ +// int ind = h*width+w; +// pcl::Normal p2 = normals_cloud->points[ind]; +// //if(w % 5 == 0 && h % 5 == 0){printf("%i %i -> curvature %f\n",w,h,p2.curvature);} +// curvaturedata[3*ind+0] = 255.0*p2.curvature; +// curvaturedata[3*ind+1] = 255.0*p2.curvature; +// curvaturedata[3*ind+2] = 255.0*p2.curvature; +// } +// } +// cv::namedWindow( "curvature", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "curvature", curvature ); +// cv::waitKey(0); + + //printf("%s LINE:%i\n",__FILE__,__LINE__); if(tune){ diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 06eb91a6..30a2fda3 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -16,6 +16,13 @@ #include "opencv2/features2d/features2d.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/calib3d/calib3d.hpp" + + +namespace gc +{ +#include "graphcuts/graph.cpp" +#include "graphcuts/maxflow.cpp" +} //#include "opencv2/nonfree/nonfree.hpp" typedef boost::property edge_weight_property; @@ -702,6 +709,7 @@ void ModelUpdater::makeInitialSetup(){ printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); + model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); printf("getSuperPoints done\n"); vector cp; @@ -714,9 +722,8 @@ printf("getSuperPoints done\n"); } //getGoodCompareFrames(cp,cf,cm); - - computeOcclusionAreas(cp, cf,cm); - +computeOcclusionAreas(cp,cf,cm); +exit(0); model->rep_relativeposes = cp; model->rep_frames = cf; model->rep_modelmasks = cm; @@ -1280,7 +1287,7 @@ void ModelUpdater::recomputeScores(){ } void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2){ - unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); + unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); float * src_normalsdata = (float *)(frame1->normals.data); @@ -1315,7 +1322,7 @@ void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weight float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - bool debugg = false; + bool debugg = false; pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); if(debugg){ @@ -1367,9 +1374,9 @@ void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weight point.x = dst_x; point.y = dst_y; point.z = dst_z; - point.r = 255; + point.r = 0; point.g = 0; - point.b = 0; + point.b = 255; dst_cloud->points[dst_ind] = point; } } @@ -1396,17 +1403,6 @@ void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weight float dst_w = dst_fx*tx*itz + dst_cx; float dst_h = dst_fy*ty*itz + dst_cy; -// pcl::PointXYZRGBNormal point; -// if(debugg){ -// point.x = tx; -// point.y = ty; -// point.z = tz; -// point.r = 255; -// point.g = 0; -// point.b = 0; -// } - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); @@ -1426,28 +1422,34 @@ void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weight float tny = m10*src_nx + m11*src_ny + m12*src_nz; float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - - double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); - d *= compare_mul; - - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - - if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE - //if(d < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE -// test_nx[ind] = test_nx.back(); test_nx.pop_back(); -// test_ny[ind] = test_ny.back(); test_ny.pop_back(); -// test_nz[ind] = test_nz.back(); test_nz.pop_back(); -// test_x[ind] = test_x.back(); test_x.pop_back(); -// test_y[ind] = test_y.back(); test_y.pop_back(); -// test_z[ind] = test_z.back(); test_z.pop_back(); -// ind--; -// if(debugg){ -// point.r = 0; -// point.g = 255; -// point.b = 0; -// } - } + double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); + //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); + //d *= src_z;//compare_mul; + //double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + overlaps1[src_ind] += weights2[dst_ind]; + overlaps2[dst_ind] += weights1[src_ind]; + + total1[src_ind] += weights2[dst_ind]; + total2[dst_ind] += weights1[src_ind]; + if(debugg){ + dst_cloud->points[dst_ind].r = 0; + dst_cloud->points[dst_ind].g = 255; + dst_cloud->points[dst_ind].b = 0; + src_cloud->points[src_ind].r = 0; + src_cloud->points[src_ind].g = 255; + src_cloud->points[src_ind].b = 0; + } + }else if(tz < dst_z){ + total1[src_ind] += weights2[dst_ind]; + total2[dst_ind] += weights1[src_ind]; + if(debugg){ + src_cloud->points[src_ind].r = 255; + src_cloud->points[src_ind].g = 0; + src_cloud->points[src_ind].b = 255; + } + } } } } @@ -1456,20 +1458,61 @@ void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weight if(debugg){ viewer->removeAllPointClouds(); - viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "cloud"); - viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "cloud"); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); viewer->spin(); } } +double interpolate( double val, double y0, double x0, double y1, double x1 ) { + return (val-x0)*(y1-y0)/(x1-x0) + y0; +} + +double base( double val ) { + if ( val <= -0.75 ) return 0; + else if ( val <= -0.25 ) return interpolate( val, 0.0, -0.75, 1.0, -0.25 ); + else if ( val <= 0.25 ) return 1.0; + else if ( val <= 0.75 ) return interpolate( val, 1.0, 0.25, 0.0, 0.75 ); + else return 0.0; +} + +double red( double gray ) { + return base( gray - 0.5 ); +} +double green( double gray ) { + return base( gray ); +} +double blue( double gray ) { + return base( gray + 0.5 ); +} + cv::Mat getImageFromArray(unsigned int width, unsigned int height, double * arr){ double maxval = 0; for(unsigned int i = 0; i < width*height; i++){ maxval = std::max(maxval,arr[i]); } + maxval = 1; + cv::Mat m; m.create(height,width,CV_8UC3); unsigned char * data = m.data; + for(unsigned int i = 0; i < width*height; i++){ + double d = arr[i]/maxval; + if(d < 0){ + data[3*i+0] = 255.0;//blue(d); + data[3*i+1] = 0;//green(d); + data[3*i+2] = 255.0;//red(d); + }else{ + data[3*i+0] = 255.0*d;//blue(d); + data[3*i+1] = 255.0*d;//green(d); + data[3*i+2] = 255.0*d;//red(d); + } + } + + cv::namedWindow( "getImageFromArray", cv::WINDOW_AUTOSIZE ); + cv::imshow( "getImageFromArray",m); + cv::waitKey(0); + return m; } @@ -1486,7 +1529,7 @@ void ModelUpdater::computeOcclusionAreas(vector cp, vector } } - weights_old.push_back(wo); + weights_old.push_back(wo); double ** overlaps = new double*[cf.size()]; double ** total = new double*[cf.size()]; @@ -1494,21 +1537,29 @@ void ModelUpdater::computeOcclusionAreas(vector cp, vector unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; overlaps[i] = new double[nr_pixels]; total[i] = new double[nr_pixels]; - for(unsigned int j = 0; j < nr_pixels; j++){ - overlaps[i][j] = 0; - total[i][j] = 0; - } } - for(unsigned int iter = 0; iter < 5; iter++){ + + for(unsigned int iter = 0; iter < 10; iter++){ + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + for(unsigned int j = 0; j < nr_pixels; j++){ + overlaps[i][j] = 0; + total[i][j] = 0; + } + } +printf("iter %i\n",iter); for(unsigned int i = 0; i < cf.size(); i++){ for(unsigned int j = 0; j < cf.size(); j++){ - //weightedocclusioncounts + if(i == j){continue;} + //weightedocclusioncounts + Eigen::Matrix4d p = cp[i].inverse() * cp[j]; + getAreaWeights(p.inverse(), cf[i], wo[i], overlaps[i], total[i], cf[j], wo[j], overlaps[j], total[j]); } } @@ -1517,26 +1568,1058 @@ void ModelUpdater::computeOcclusionAreas(vector cp, vector unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; wo[i] = new double[nr_pixels]; for(unsigned int j = 0; j < nr_pixels; j++){ - if(total[i][j] == 0){ - wo[i][j] = 0.0; + if(total[i][j] < 0.00001){ + wo[i][j] = 0.0; }else{ - wo[i][j] = overlaps[i][j]/total[i][j]; - } - } - - double sum = 0; - for(unsigned int j = 0; j < nr_pixels; j++){ - sum += wo[i][j]; - } - sum /= double(nr_pixels); - for(unsigned int j = 0; j < nr_pixels; j++){ - if(total[i][j] == 0){ - wo[i][j] = sum; + double fails = occlusion_penalty*(total[i][j]-overlaps[i][j]); + double others = overlaps[i][j]; + wo[i][j] = others/(fails+others);//overlaps[i][j]/total[i][j]; + //wo[i][j] = wo[i][j]*0.9 + weights_old.back()[i][j]*0.9; } } +//if(iter % 5 == 0){getImageFromArray(cf[i]->camera->width, cf[i]->camera->height, wo[i]);} +// printf("%i / %i \n",i+1,cf.size()); +// cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "depthedges", getImageFromArray(cf[i]->camera->width, cf[i]->camera->height, wo[i]) ); +// cv::waitKey(0); } weights_old.push_back(wo); } + + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + double sum = 0; + for(unsigned int j = 0; j < nr_pixels; j++){ + sum += wo[i][j]; + } + sum /= double(nr_pixels); + for(unsigned int j = 0; j < nr_pixels; j++){ + if(total[i][j] == 0){ + wo[i][j] = sum; + } + } + } + for(unsigned int i = 0; i < cf.size(); i++){ + for(unsigned int j = 0; j < weights_old.size(); j++){ + getImageFromArray(cf[i]->camera->width, cf[i]->camera->height, weights_old[j][i]); + } + } +} + + +void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, cv::Mat mask){ + unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); + float * src_normalsdata = (float *)(frame1->normals.data); + + Camera * src_camera = frame1->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); + float * dst_normalsdata = (float *)(frame2->normals.data); + unsigned char * dst_maskdata = (unsigned char *)(mask.data); + + Camera * dst_camera = frame2->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + bool debugg = false; + pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); + if(debugg){ + + src_cloud->points.resize(src_width*src_height); + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + point.r = 255; + point.g = 0; + point.b = 0; + src_cloud->points[src_ind] = point; + } + } + } + + dst_cloud->points.resize(dst_width*dst_height); + for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + pcl::PointXYZRGBNormal point; + point.x = dst_x; + point.y = dst_y; + point.z = dst_z; + point.r = 0; + point.g = 0; + point.b = 255; + dst_cloud->points[dst_ind] = point; + } + } + } + } + + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + double dst_noise = dst_z * dst_z; + double point_noise = src_z * src_z; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); + //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); + //d *= src_z;//compare_mul; + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + if(surface_angle > 0.8 && dst_maskdata[dst_ind] > 0){ + overlaps[src_ind]++; + if(debugg){ + dst_cloud->points[dst_ind].r = 0; + dst_cloud->points[dst_ind].g = 255; + dst_cloud->points[dst_ind].b = 0; + src_cloud->points[src_ind].r = 0; + src_cloud->points[src_ind].g = 255; + src_cloud->points[src_ind].b = 0; + } + } + }else if(tz < dst_z){ + occlusions[src_ind]++; + if(debugg){ + src_cloud->points[src_ind].r = 255; + src_cloud->points[src_ind].g = 0; + src_cloud->points[src_ind].b = 255; + } + } + } + } + } + } + } + + if(debugg){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); + viewer->spin(); + } +} + +std::string type2str(int type) { + string r; + + uchar depth = type & CV_MAT_DEPTH_MASK; + uchar chans = 1 + (type >> CV_CN_SHIFT); + + switch ( depth ) { + case CV_8U: r = "8U"; break; + case CV_8S: r = "8S"; break; + case CV_16U: r = "16U"; break; + case CV_16S: r = "16S"; break; + case CV_32S: r = "32S"; break; + case CV_32F: r = "32F"; break; + case CV_64F: r = "64F"; break; + default: r = "User"; break; + } + + r += "C"; + r += (chans+'0'); + + return r; +} + +/** + * Code for thinning a binary image using Zhang-Suen algorithm. + * + * Author: Nash (nash [at] opencv-code [dot] com) + * Website: http://opencv-code.com + */ +#include + +/** + * Perform one thinning iteration. + * Normally you wouldn't call this function directly from your code. + * + * Parameters: + * im Binary image with range = [0,1] + * iter 0=even, 1=odd + */ +void thinningIteration(cv::Mat& img, int iter) +{ + CV_Assert(img.channels() == 1); + CV_Assert(img.depth() != sizeof(uchar)); + CV_Assert(img.rows > 3 && img.cols > 3); + + cv::Mat marker = cv::Mat::zeros(img.size(), CV_8UC1); + + int nRows = img.rows; + int nCols = img.cols; + + if (img.isContinuous()) { + nCols *= nRows; + nRows = 1; + } + + int x, y; + uchar *pAbove; + uchar *pCurr; + uchar *pBelow; + uchar *nw, *no, *ne; // north (pAbove) + uchar *we, *me, *ea; + uchar *sw, *so, *se; // south (pBelow) + + uchar *pDst; + + // initialize row pointers + pAbove = NULL; + pCurr = img.ptr(0); + pBelow = img.ptr(1); + + for (y = 1; y < img.rows-1; ++y) { + // shift the rows up by one + pAbove = pCurr; + pCurr = pBelow; + pBelow = img.ptr(y+1); + + pDst = marker.ptr(y); + + // initialize col pointers + no = &(pAbove[0]); + ne = &(pAbove[1]); + me = &(pCurr[0]); + ea = &(pCurr[1]); + so = &(pBelow[0]); + se = &(pBelow[1]); + + for (x = 1; x < img.cols-1; ++x) { + // shift col pointers left by one (scan left to right) + nw = no; + no = ne; + ne = &(pAbove[x+1]); + we = me; + me = ea; + ea = &(pCurr[x+1]); + sw = so; + so = se; + se = &(pBelow[x+1]); + + int A = (*no == 0 && *ne == 1) + (*ne == 0 && *ea == 1) + + (*ea == 0 && *se == 1) + (*se == 0 && *so == 1) + + (*so == 0 && *sw == 1) + (*sw == 0 && *we == 1) + + (*we == 0 && *nw == 1) + (*nw == 0 && *no == 1); + int B = *no + *ne + *ea + *se + *so + *sw + *we + *nw; + int m1 = iter == 0 ? (*no * *ea * *so) : (*no * *ea * *we); + int m2 = iter == 0 ? (*ea * *so * *we) : (*no * *so * *we); + + if (A == 1 && (B >= 2 && B <= 6) && m1 == 0 && m2 == 0) + pDst[x] = 1; + } + } + + img &= ~marker; +} + +/** + * Function for thinning the given binary image + * + * Parameters: + * src The source image, binary with range = [0,255] + * dst The destination image + */ +void thinning(const cv::Mat& src, cv::Mat& dst) +{ + dst = src.clone(); + dst /= 255; // convert to binary image + + cv::Mat prev = cv::Mat::zeros(dst.size(), CV_8UC1); + cv::Mat diff; + + do { + thinningIteration(dst, 0); + thinningIteration(dst, 1); + cv::absdiff(dst, prev, diff); + dst.copyTo(prev); + } + while (cv::countNonZero(diff) > 0); + + dst *= 255; +} + +void compute_thin_edges(std::vector< std::vector > probs,reglib::RGBDFrame * frame, int blursize = 5, double threshold = 0.5){ + unsigned int width = frame->rgb.cols; + unsigned int height = frame->rgb.rows; + + std::vector totprob; totprob.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totprob[i] = 0.5;} + std::vector totnprob; totnprob.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totnprob[i] = 0.5;} + for(unsigned int p = 0; p < probs.size()-2; p++){ + for(unsigned int i = 0; i < width*height;i++){ + totprob[i] *= probs[p][i]; + totnprob[i] *= 1.0- probs[p][i]; + } + } + + cv::Mat joint_prob; + joint_prob.create(height,width,CV_8UC1); + unsigned char * jointdata_prob = (unsigned char *)(joint_prob.data); + for(unsigned int i = 0; i < width*height;i++){ + jointdata_prob[i] = 255.0*((totprob[i]/(totprob[i]+totnprob[i])) < threshold); + } + + cv::imshow( "rgb", frame->rgb ); + cv::imshow( "joint_prob", joint_prob ); + cv::waitKey(50); + + int dilation_size = 1; + cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ); + cv::dilate( joint_prob, joint_prob, element ); + cv::imshow( "dilate", joint_prob ); + cv::waitKey(50); + + thinning(joint_prob, joint_prob); + cv::imshow( "skeleton", joint_prob ); + cv::waitKey(0); +} + +std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ + cv::Mat src = frame->rgb.clone(); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + const float idepth = frame->camera->idepth_scale; + + cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); + + unsigned char * srcdata = (unsigned char *)src.data; + unsigned int width = src.cols; + unsigned int height = src.rows; + unsigned int nr_pixels = width*height; + unsigned int nr_rgbedges = (width-2)*height + width*(height-2) + 2*(width-2)*(height-2); + + std::vector< std::vector > probs; + bool cross = false; + unsigned int chans = 3; + for(unsigned int c = 0; c < chans;c++){ + std::vector Xvec; + int dir; + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + dir = 1; + //Xvec.push_back(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = width; + //Xvec.push_back(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); +/* + dir = 1+width; + //Xvec.push_back(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = 1-width; + //Xvec.push_back(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + */ + } + } + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + func->zeromean = true; + func->maxp = 0.99; + func->startreg = 0.0; + func->debugg_print = false; + func->bidir = true; + func->maxd = 256.0; + func->histogram_size = 256; + func->fixed_histogram_size = true; + func->startmaxd = func->maxd; + func->starthistogram_size = func->histogram_size; + func->blurval = 0.005; + func->stdval2 = stdval; + func->maxnoise = stdval; + func->reset(); + func->computeModel(X); + printf("noise: %5.5f\n",func->getNoise()); + + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + std::vector dxy; dxy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dxy[i] = 0.5;} + std::vector dyx; dyx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dyx[i] = 0.5;} + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; +// dir = 1; +// dx[ind] = func->getProb(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); + +// dir = width; +// dy[ind] = func->getProb(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); + +// dir = 1+width; +// dxy[ind] = func->getProb(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); + +// dir = 1-width; +// dyx[ind] = func->getProb(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); + + dir = 1; + dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = width; + dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = 1+width; + dxy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = 1-width; + dyx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + } + } + delete func; + + probs.push_back(dx); + probs.push_back(dy); +// probs.push_back(dxy); +// probs.push_back(dyx); + } +/* + std::vector< std::vector > probs2; + for(unsigned int c = 0; c < 3;c++){ + std::vector Xvec; + for(unsigned int w = 10; w < width;w++){ + for(unsigned int h = 10; h < height-1;h++){ + int ind = h*width+w; + if(fabs(normalsdata[3*ind]) > 1 && fabs(normalsdata[3*(ind-10)]) > 1 && fabs(normalsdata[3*(ind-10*width)]) > 1){continue;} + Xvec.push_back(fabs(normalsdata[3*ind+c] - normalsdata[3*(ind-10*1)+c])); + Xvec.push_back(fabs(normalsdata[3*ind+c] - normalsdata[3*(ind-10*width)+c])); + } + } + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); + funcZ->zeromean = true; + funcZ->startreg = 0.025; + funcZ->debugg_print = true; + funcZ->bidir = true; + funcZ->maxd = 1.00; + funcZ->histogram_size = 1000; + funcZ->fixed_histogram_size = true; + funcZ->startmaxd = funcZ->maxd; + funcZ->starthistogram_size = funcZ->histogram_size; + funcZ->blurval = 0.5; + funcZ->stdval2 = stdval; + funcZ->maxnoise = stdval; + funcZ->reset(); + funcZ->computeModel(X); + printf("noise: %5.5f\n",funcZ->getNoise()); + + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + for(unsigned int w = 10; w < width;w++){ + for(unsigned int h = 10; h < height-1;h++){ + int ind = h*width+w; + if(fabs(normalsdata[3*ind]) > 1 && fabs(normalsdata[3*(ind-10*1)]) > 1 && fabs(normalsdata[3*(ind-10*width)]) > 1){continue;} + dx[ind] = funcZ->getProb(fabs(normalsdata[3*ind+c] - normalsdata[3*(ind-10*1)+c])); + dy[ind] = funcZ->getProb(fabs(normalsdata[3*ind+c] - normalsdata[3*(ind-10*width)+c])); + } + } + delete funcZ; + + probs2.push_back(dx); + probs2.push_back(dy); + } + + std::vector totprob2; totprob2.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totprob2[i] = 0.5;} + std::vector totnprob2; totnprob2.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totnprob2[i] = 0.5;} + for(unsigned int p = 0; p < probs2.size(); p++){ + for(unsigned int i = 0; i < width*height;i++){ + totprob2[i] *= probs2[p][i]; + totnprob2[i] *= 1.0- probs2[p][i]; + } + + cv::Mat joint_prob; + joint_prob.create(height,width,CV_8UC3); + unsigned char * jointdata_prob = (unsigned char *)(joint_prob.data); + for(unsigned int i = 0; i < width*height;i++){ + jointdata_prob[3*i+0] = 255.0*totprob2[i]/(totprob2[i]+totnprob2[i]); + jointdata_prob[3*i+1] = 255.0*totprob2[i]/(totprob2[i]+totnprob2[i]); + jointdata_prob[3*i+2] = 255.0*totprob2[i]/(totprob2[i]+totnprob2[i]); + } + cv::imshow( "rgb", src ); + cv::imshow( "joint_prob", joint_prob ); + cv::waitKey(0); + } + +// cv::Mat joint_prob; +// joint_prob.create(height,width,CV_8UC3); +// unsigned char * jointdata_prob = (unsigned char *)(joint_prob.data); +// for(unsigned int i = 0; i < width*height;i++){ +// jointdata_prob[3*i+0] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); +// jointdata_prob[3*i+1] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); +// jointdata_prob[3*i+2] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); +// } + +// cv::imshow( "rgb", src ); +// cv::imshow( "joint_prob", joint_prob ); +// cv::waitKey(0); + +*/ + + + + { + std::vector Xvec; + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + } + } + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); + funcZ->zeromean = true; + funcZ->startreg = 0.002; + funcZ->debugg_print = false; + funcZ->bidir = true; + funcZ->maxp = 0.999999; + funcZ->maxd = 0.1; + funcZ->histogram_size = 100; + funcZ->fixed_histogram_size = true; + funcZ->startmaxd = funcZ->maxd; + funcZ->starthistogram_size = funcZ->histogram_size; + funcZ->blurval = 0.5; + funcZ->stdval2 = stdval; + funcZ->maxnoise = stdval; + funcZ->reset(); + funcZ->computeModel(X); + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + } + } + + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + if(z3 > 0){z2 = 2*z2-z3;} + + if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb((z-z2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + if(z3 > 0){z2 = 2*z2-z3;} + + if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb((z-z2)/(z*z+z2*z2));} + } + } + } + + delete funcZ; + probs.push_back(dx); + probs.push_back(dy); + } + +// std::vector totprob; totprob.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totprob[i] = 0.5;} +// std::vector totnprob; totnprob.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totnprob[i] = 0.5;} +// for(unsigned int p = 0; p < probs.size(); p++){ +// for(unsigned int i = 0; i < width*height;i++){ +// totprob[i] *= probs[p][i]; +// totnprob[i] *= 1.0- probs[p][i]; +// } +// } + +// cv::Mat joint_prob; +// joint_prob.create(height,width,CV_8UC3); +// unsigned char * jointdata_prob = (unsigned char *)(joint_prob.data); +// for(unsigned int i = 0; i < width*height;i++){ +// jointdata_prob[3*i+0] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); +// jointdata_prob[3*i+1] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); +// jointdata_prob[3*i+2] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); +// } + +// cv::imshow( "rgb", src ); +// cv::imshow( "joint_prob", joint_prob ); +// cv::waitKey(0); + + return probs; +} + +std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, vector cp, vector cf, vector oldmasks){ + std::vector newmasks; + for(unsigned int i = 0; i < cf.size(); i++){ + + unsigned char * rgbdata = (unsigned char *)(cf[i]->rgb.data); + unsigned short * depthdata = (unsigned short *)(cf[i]->depth.data); + float * normalsdata = (float *)(cf[i]->normals.data); + + + Camera * camera = cf[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + double * overlaps = new double[nr_pixels]; + double * occlusions = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + overlaps[j] = 0; + occlusions[j] = 0; + } + + for(unsigned int j = 0; j < cf.size(); j++){ + if(i == j){continue;} + Eigen::Matrix4d p = cp[i].inverse() * cp[j]; + getDynamicWeights(p.inverse(), cf[i], overlaps, occlusions, cf[j],oldmasks[j]); + } + std::vector< std::vector > probs = getImageProbs(cf[i],5); + + printf("starting partition\n"); + double start_part = getTime(); + int nr_data = width*height; + gc::Graph *g = new gc::Graph( nr_data, width*(height-1) + (width-1)*height); + + double maxprob = 0.7; + for(unsigned int j = 0; j < nr_data;j++){ + g -> add_node(); + if(depthdata[j] == 0){ + g -> add_tweights(j,0,0); + continue; + } + + double p_fg = 0.499; + + if(occlusions[j] >= 1){ p_fg = maxprob;} + else if(overlaps[j] >= 1){ p_fg = 0.4;} + + p_fg = std::max(1-maxprob,std::min(maxprob,p_fg)); + + double p_bg = 1-p_fg; + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + g -> add_tweights( j, weightFG, weightBG ); + } + + double maxprob_same = 0.999999999; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + + double ax = 0.5; + double bx = 0.5; + for(int p = 0; p < probs.size(); p+=2){ + double pr = probs[p][ind]; + ax *= pr; + bx *= 1.0-pr; + } + double px = ax/(ax+bx); + + double ay = 0.5; + double by = 0.5; + for(int p = 1; p < probs.size(); p+=2){ + double pr = probs[p][ind]; + ay *= pr; + by *= 1.0-pr; + } + double py = ay/(ay+by); + + if(w > 0){ + int other = ind-1; + double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + if(w % 10 == 0 && h % 10 == 0){printf("%i %i -> P:%10.10f -> weight: %10.10f\n",w,h,p_same,weight);} + g -> add_edge( ind, other, weight, weight ); + } + + if(h > 0){ + int other = ind-width; + double p_same = std::max(probs[probs.size()-1][ind],std::min(py,maxprob_same)); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + g -> add_edge( ind, other, weight, weight ); + } + } + } + + int flow = g -> maxflow(); + printf("flow: %i\n",flow); + + + double end_part = getTime(); + printf("part time: %10.10fs\n",end_part-start_part); + + cv::Mat internalmask; + + internalmask.create(height,width,CV_8UC1); + unsigned char * internaldata = (unsigned char *)(internalmask.data); + for(unsigned int i = 0; i < width*height;i++){ + internaldata[i] = 255.0*(g->what_segment(i) == gc::Graph::SOURCE); + } + + cv::imshow( "rgb", cf[i]->rgb ); + cv::imshow( "internalmask", internalmask ); + cv::waitKey(0); + + newmasks.push_back(internalmask); + //Time to compute external masks... + + bool debugg1 = false; + if(debugg1){ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->points.resize(width*height); + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + pcl::PointXYZRGBNormal point; + point.x = x; + point.y = y; + point.z = z; + if (g->what_segment(ind) == gc::Graph::SOURCE){ + point.r = 0; + point.g = 255; + point.b = 0; + }else{ + point.r = 255; + point.g = 0; + point.b = 0; + } + cloud->points[ind] = point; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + viewer->spin(); + } + delete g; + + bool debugg2 = false; + if(debugg2){ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->points.resize(width*height); + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + float nx = normalsdata[3*ind+0]; + if(z > 0 && nx != 2){ + float ny = normalsdata[3*ind+1]; + float nz = normalsdata[3*ind+2]; + + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + + pcl::PointXYZRGBNormal point; + point.x = x; + point.y = y; + point.z = z; + point.r = 0; + point.g = 255; + point.b = 255; + + if(overlaps[ind] >= 1){ + point.r = 0; + point.g = 255; + point.b = 0; + } + if(occlusions[ind] >= 1){ + point.r = 255; + point.g = 0; + point.b = 0; + } + cloud->points[ind] = point; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + viewer->spin(); + } + + bool debugg3 = false; + if(debugg3){ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->points.resize(width*height); + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + double ax = 0.5; + double bx = 0.5; + for(int p = 0; p < probs.size()-2; p+=2){ + double pr = probs[p][ind]; + ax *= pr; + bx *= 1.0-pr; + } + double px = ax/(ax+bx); + + double ay = 0.5; + double by = 0.5; + for(int p = 1; p < probs.size()-1; p+=2){ + double pr = probs[p][ind]; + ay *= pr; + by *= 1.0-pr; + } + double py = ay/(ay+by); + + double p_same_color = std::max(0.25, std::min(px,maxprob_same)); + double p_same_depth = std::min(probs[probs.size()-2][ind], maxprob_same); + double not_p_same_color = 1-p_same_color; + double not_p_same_depth = 1-p_same_depth; + double not_p_same = std::min(not_p_same_color,not_p_same_depth); + +// double ptest = (ax*ay)/(ax*ay+bx*by); + + pcl::PointXYZRGBNormal point; + point.x = x; + point.y = y; + point.z = z; + point.r = 255*not_p_same_color;//(1.0-probs[0][ind]); + point.g = 255*not_p_same_color;//*(1.0-probs[1][ind]); + point.b = 255*not_p_same_color; + cloud->points[ind] = point; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + viewer->spin(); + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + double ax = 0.5; + double bx = 0.5; + for(int p = 0; p < probs.size()-2; p+=2){ + double pr = probs[p][ind]; + ax *= pr; + bx *= 1.0-pr; + } + double px = ax/(ax+bx); + + double ay = 0.5; + double by = 0.5; + for(int p = 1; p < probs.size()-1; p+=2){ + double pr = probs[p][ind]; + ay *= pr; + by *= 1.0-pr; + } + double py = ay/(ay+by); + + double p_same_color = std::max(0.25, std::min(py,maxprob_same)); + double p_same_depth = std::min(probs[probs.size()-1][ind], maxprob_same); + double not_p_same_color = 1-p_same_color; + double not_p_same_depth = 1-p_same_depth; + double not_p_same = std::min(not_p_same_color,not_p_same_depth); + + + pcl::PointXYZRGBNormal point; + point.x = x; + point.y = y; + point.z = z; + point.r = 255*not_p_same_color;//(1.0-probs[0][ind]); + point.g = 255*not_p_same_color;//*(1.0-probs[1][ind]); + point.b = 255*not_p_same_color; + cloud->points[ind] = point; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + viewer->spin(); + } + + delete[] overlaps; + delete[] occlusions; + } } OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * src_modelmask, RGBDFrame * dst, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step, bool debugg){ diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/CHANGES.TXT b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/CHANGES.TXT new file mode 100644 index 00000000..edd859d1 --- /dev/null +++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/CHANGES.TXT @@ -0,0 +1,29 @@ +List of changes from version 3.0: +- Moved line + #include "instances.inc" +to the end of cpp files to make it compile under GNU c++ compilers 4.2(?) and above + +List of changes from version 2.2: + +- Added functions for accessing graph structure, residual capacities, etc. + (They are needed for implementing maxflow-based algorithms such as primal-dual algorithm for convex MRFs.) +- Added option of reusing trees. +- node_id's are now integers starting from 0. Thus, it is not necessary to store node_id's in a separate array. +- Capacity types are now templated. +- Fixed bug in block.h. (After Block::Reset, ScanFirst() and ScanNext() did not work properly). +- Implementation with a forward star representation of the graph is no longer supported. (It needs less memory, but slightly slower than adjacency list representation.) If you still wish to use it, download version 2.2. +- Note: version 3.0 is released under a different license than version 2.2. + +List of changes from version 2.1: + +- Put the code under GPL license + +List of changes from version 2.02: + +- Fixed a bug in the implementation that uses forward star representation + +List of changes from version 2.01: + +- Added new interface function - Graph::add_tweights(Node_id, captype, captype) + (necessary for the "ENERGY" software package) + diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/README.TXT b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/README.TXT new file mode 100644 index 00000000..367afefe --- /dev/null +++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/README.TXT @@ -0,0 +1,123 @@ +################################################################### +# # +# MAXFLOW - software for computing mincut/maxflow in a graph # +# Version 3.01 # +# http://www.cs.ucl.ac.uk/staff/V.Kolmogorov/software.html # +# # +# Yuri Boykov (yuri@csd.uwo.ca) # +# Vladimir Kolmogorov (v.kolmogorov@cs.ucl.ac.uk) # +# 2001-2006 # +# # +################################################################### + +1. Introduction. + +This software library implements the maxflow algorithm described in + + "An Experimental Comparison of Min-Cut/Max-Flow Algorithms for Energy Minimization in Vision." + Yuri Boykov and Vladimir Kolmogorov. + In IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI), + September 2004 + +This algorithm was developed by Yuri Boykov and Vladimir Kolmogorov +at Siemens Corporate Research. To make it available for public use, +it was later reimplemented by Vladimir Kolmogorov based on open publications. + +If you use this software for research purposes, you should cite +the aforementioned paper in any resulting publication. + +---------------------------------------------------------------------- + +REUSING TREES: + +Starting with version 3.0, there is a also an option of reusing search +trees from one maxflow computation to the next, as described in + + "Efficiently Solving Dynamic Markov Random Fields Using Graph Cuts." + Pushmeet Kohli and Philip H.S. Torr + International Conference on Computer Vision (ICCV), 2005 + +If you use this option, you should cite +the aforementioned paper in any resulting publication. + +Tested under windows, Visual C++ 6.0 compiler and unix (SunOS 5.8 +and RedHat Linux 7.0, GNU c++ compiler). + +################################################################## + +2. License & disclaimer. + + Copyright 2001-2006 Vladimir Kolmogorov (v.kolmogorov@cs.ucl.ac.uk), Yuri Boykov (yuri@csd.uwo.ca). + + This software can be used for research purposes only. + If you require another license, you may consider using version 2.21 + (which implements exactly the same algorithm, but does not have the option of reusing search trees). + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +################################################################## + +3. Example usage. + +This section shows how to use the library to compute +a minimum cut on the following graph: + + SOURCE + / \ + 1/ \2 + / 3 \ + node0 -----> node1 + | <----- | + | 4 | + \ / + 5\ /6 + \ / + SINK + +/////////////////////////////////////////////////// + +#include +#include "graph.h" + +int main() +{ + typedef Graph GraphType; + GraphType *g = new GraphType(/*estimated # of nodes*/ 2, /*estimated # of edges*/ 1); + + g -> add_node(); + g -> add_node(); + + g -> add_tweights( 0, /* capacities */ 1, 5 ); + g -> add_tweights( 1, /* capacities */ 2, 6 ); + g -> add_edge( 0, 1, /* capacities */ 3, 4 ); + + int flow = g -> maxflow(); + + printf("Flow = %d\n", flow); + printf("Minimum cut:\n"); + if (g->what_segment(0) == GraphType::SOURCE) + printf("node0 is in the SOURCE set\n"); + else + printf("node0 is in the SINK set\n"); + if (g->what_segment(1) == GraphType::SOURCE) + printf("node1 is in the SOURCE set\n"); + else + printf("node1 is in the SINK set\n"); + + delete g; + + return 0; +} + + +/////////////////////////////////////////////////// diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/block.h b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/block.h new file mode 100644 index 00000000..c4dfa467 --- /dev/null +++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/block.h @@ -0,0 +1,268 @@ +/* block.h */ +/* + Template classes Block and DBlock + Implement adding and deleting items of the same type in blocks. + + If there there are many items then using Block or DBlock + is more efficient than using 'new' and 'delete' both in terms + of memory and time since + (1) On some systems there is some minimum amount of memory + that 'new' can allocate (e.g., 64), so if items are + small that a lot of memory is wasted. + (2) 'new' and 'delete' are designed for items of varying size. + If all items has the same size, then an algorithm for + adding and deleting can be made more efficient. + (3) All Block and DBlock functions are inline, so there are + no extra function calls. + + Differences between Block and DBlock: + (1) DBlock allows both adding and deleting items, + whereas Block allows only adding items. + (2) Block has an additional operation of scanning + items added so far (in the order in which they were added). + (3) Block allows to allocate several consecutive + items at a time, whereas DBlock can add only a single item. + + Note that no constructors or destructors are called for items. + + Example usage for items of type 'MyType': + + /////////////////////////////////////////////////// + #include "block.h" + #define BLOCK_SIZE 1024 + typedef struct { int a, b; } MyType; + MyType *ptr, *array[10000]; + + ... + + Block *block = new Block(BLOCK_SIZE); + + // adding items + for (int i=0; i New(); + ptr -> a = ptr -> b = rand(); + } + + // reading items + for (ptr=block->ScanFirst(); ptr; ptr=block->ScanNext()) + { + printf("%d %d\n", ptr->a, ptr->b); + } + + delete block; + + ... + + DBlock *dblock = new DBlock(BLOCK_SIZE); + + // adding items + for (int i=0; i New(); + } + + // deleting items + for (int i=0; i Delete(array[i]); + } + + // adding items + for (int i=0; i New(); + } + + delete dblock; + + /////////////////////////////////////////////////// + + Note that DBlock deletes items by marking them as + empty (i.e., by adding them to the list of free items), + so that this memory could be used for subsequently + added items. Thus, at each moment the memory allocated + is determined by the maximum number of items allocated + simultaneously at earlier moments. All memory is + deallocated only when the destructor is called. +*/ + +#ifndef __BLOCK_H__ +#define __BLOCK_H__ + +#include + +/***********************************************************************/ +/***********************************************************************/ +/***********************************************************************/ + +template class Block +{ +public: + /* Constructor. Arguments are the block size and + (optionally) the pointer to the function which + will be called if allocation failed; the message + passed to this function is "Not enough memory!" */ + Block(int size, void (*err_function)(char *) = NULL) { first = last = NULL; block_size = size; error_function = err_function; } + + /* Destructor. Deallocates all items added so far */ + ~Block() { while (first) { block *next = first -> next; delete[] ((char*)first); first = next; } } + + /* Allocates 'num' consecutive items; returns pointer + to the first item. 'num' cannot be greater than the + block size since items must fit in one block */ + Type *New(int num = 1) + { + Type *t; + + if (!last || last->current + num > last->last) + { + if (last && last->next) last = last -> next; + else + { + block *next = (block *) new char [sizeof(block) + (block_size-1)*sizeof(Type)]; + if (!next) { if (error_function) (*error_function)("Not enough memory!"); exit(1); } + if (last) last -> next = next; + else first = next; + last = next; + last -> current = & ( last -> data[0] ); + last -> last = last -> current + block_size; + last -> next = NULL; + } + } + + t = last -> current; + last -> current += num; + return t; + } + + /* Returns the first item (or NULL, if no items were added) */ + Type *ScanFirst() + { + for (scan_current_block=first; scan_current_block; scan_current_block = scan_current_block->next) + { + scan_current_data = & ( scan_current_block -> data[0] ); + if (scan_current_data < scan_current_block -> current) return scan_current_data ++; + } + return NULL; + } + + /* Returns the next item (or NULL, if all items have been read) + Can be called only if previous ScanFirst() or ScanNext() + call returned not NULL. */ + Type *ScanNext() + { + while (scan_current_data >= scan_current_block -> current) + { + scan_current_block = scan_current_block -> next; + if (!scan_current_block) return NULL; + scan_current_data = & ( scan_current_block -> data[0] ); + } + return scan_current_data ++; + } + + /* Marks all elements as empty */ + void Reset() + { + block *b; + if (!first) return; + for (b=first; ; b=b->next) + { + b -> current = & ( b -> data[0] ); + if (b == last) break; + } + last = first; + } + +/***********************************************************************/ + +private: + + typedef struct block_st + { + Type *current, *last; + struct block_st *next; + Type data[1]; + } block; + + int block_size; + block *first; + block *last; + + block *scan_current_block; + Type *scan_current_data; + + void (*error_function)(char *); +}; + +/***********************************************************************/ +/***********************************************************************/ +/***********************************************************************/ + +template class DBlock +{ +public: + /* Constructor. Arguments are the block size and + (optionally) the pointer to the function which + will be called if allocation failed; the message + passed to this function is "Not enough memory!" */ + DBlock(int size, void (*err_function)(char *) = NULL) { first = NULL; first_free = NULL; block_size = size; error_function = err_function; } + + /* Destructor. Deallocates all items added so far */ + ~DBlock() { while (first) { block *next = first -> next; delete[] ((char*)first); first = next; } } + + /* Allocates one item */ + Type *New() + { + block_item *item; + + if (!first_free) + { + block *next = first; + first = (block *) new char [sizeof(block) + (block_size-1)*sizeof(block_item)]; + if (!first) { if (error_function) (*error_function)("Not enough memory!"); exit(1); } + first_free = & (first -> data[0] ); + for (item=first_free; item next_free = item + 1; + item -> next_free = NULL; + first -> next = next; + } + + item = first_free; + first_free = item -> next_free; + return (Type *) item; + } + + /* Deletes an item allocated previously */ + void Delete(Type *t) + { + ((block_item *) t) -> next_free = first_free; + first_free = (block_item *) t; + } + +/***********************************************************************/ + +private: + + typedef union block_item_st + { + Type t; + block_item_st *next_free; + } block_item; + + typedef struct block_st + { + struct block_st *next; + block_item data[1]; + } block; + + int block_size; + block *first; + block_item *first_free; + + void (*error_function)(char *); +}; + + +#endif + diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.cpp b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.cpp new file mode 100644 index 00000000..fad38542 --- /dev/null +++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.cpp @@ -0,0 +1,115 @@ +/* graph.cpp */ + + +#include +#include +#include +#include "graph.h" + + +template + Graph::Graph(int node_num_max, int edge_num_max, void (*err_function)(char *)) + : node_num(0), + nodeptr_block(NULL), + error_function(err_function) +{ + if (node_num_max < 16) node_num_max = 16; + if (edge_num_max < 16) edge_num_max = 16; + + nodes = (node*) malloc(node_num_max*sizeof(node)); + arcs = (arc*) malloc(2*edge_num_max*sizeof(arc)); + if (!nodes || !arcs) { if (error_function) (*error_function)("Not enough memory!"); exit(1); } + + node_last = nodes; + node_max = nodes + node_num_max; + arc_last = arcs; + arc_max = arcs + 2*edge_num_max; + + maxflow_iteration = 0; + flow = 0; +} + +template + Graph::~Graph() +{ + if (nodeptr_block) + { + delete nodeptr_block; + nodeptr_block = NULL; + } + free(nodes); + free(arcs); +} + +template + void Graph::reset() +{ + node_last = nodes; + arc_last = arcs; + node_num = 0; + + if (nodeptr_block) + { + delete nodeptr_block; + nodeptr_block = NULL; + } + + maxflow_iteration = 0; + flow = 0; +} + +template + void Graph::reallocate_nodes(int num) +{ + int node_num_max = (int)(node_max - nodes); + node* nodes_old = nodes; + + node_num_max += node_num_max / 2; + if (node_num_max < node_num + num) node_num_max = node_num + num; + nodes = (node*) realloc(nodes_old, node_num_max*sizeof(node)); + if (!nodes) { if (error_function) (*error_function)("Not enough memory!"); exit(1); } + + node_last = nodes + node_num; + node_max = nodes + node_num_max; + + if (nodes != nodes_old) + { + arc* a; + for (a=arcs; ahead = (node*) ((char*)a->head + (((char*) nodes) - ((char*) nodes_old))); + } + } +} + +template + void Graph::reallocate_arcs() +{ + int arc_num_max = (int)(arc_max - arcs); + int arc_num = (int)(arc_last - arcs); + arc* arcs_old = arcs; + + arc_num_max += arc_num_max / 2; if (arc_num_max & 1) arc_num_max ++; + arcs = (arc*) realloc(arcs_old, arc_num_max*sizeof(arc)); + if (!arcs) { if (error_function) (*error_function)("Not enough memory!"); exit(1); } + + arc_last = arcs + arc_num; + arc_max = arcs + arc_num_max; + + if (arcs != arcs_old) + { + node* i; + arc* a; + for (i=nodes; ifirst) i->first = (arc*) ((char*)i->first + (((char*) arcs) - ((char*) arcs_old))); + } + for (a=arcs; anext) a->next = (arc*) ((char*)a->next + (((char*) arcs) - ((char*) arcs_old))); + a->sister = (arc*) ((char*)a->sister + (((char*) arcs) - ((char*) arcs_old))); + } + } +} + +#include "instances.inc" diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.h b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.h new file mode 100644 index 00000000..87efe7ef --- /dev/null +++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/graph.h @@ -0,0 +1,506 @@ +/* graph.h */ +/* + This software library implements the maxflow algorithm + described in + + "An Experimental Comparison of Min-Cut/Max-Flow Algorithms for Energy Minimization in Vision." + Yuri Boykov and Vladimir Kolmogorov. + In IEEE Transactions on Pattern Analysis and Machine Intelligence (PAMI), + September 2004 + + This algorithm was developed by Yuri Boykov and Vladimir Kolmogorov + at Siemens Corporate Research. To make it available for public use, + it was later reimplemented by Vladimir Kolmogorov based on open publications. + + If you use this software for research purposes, you should cite + the aforementioned paper in any resulting publication. + + ---------------------------------------------------------------------- + + REUSING TREES: + + Starting with version 3.0, there is a also an option of reusing search + trees from one maxflow computation to the next, as described in + + "Efficiently Solving Dynamic Markov Random Fields Using Graph Cuts." + Pushmeet Kohli and Philip H.S. Torr + International Conference on Computer Vision (ICCV), 2005 + + If you use this option, you should cite + the aforementioned paper in any resulting publication. +*/ + + + +/* + For description, license, example usage see README.TXT. +*/ + +#ifndef __GRAPH_H__ +#define __GRAPH_H__ + +#include +#include "block.h" + +#include +// NOTE: in UNIX you need to use -DNDEBUG preprocessor option to supress assert's!!! + + + +// captype: type of edge capacities (excluding t-links) +// tcaptype: type of t-links (edges between nodes and terminals) +// flowtype: type of total flow +// +// Current instantiations are in instances.inc +template class Graph +{ +public: + typedef enum + { + SOURCE = 0, + SINK = 1 + } termtype; // terminals + typedef int node_id; + + ///////////////////////////////////////////////////////////////////////// + // BASIC INTERFACE FUNCTIONS // + // (should be enough for most applications) // + ///////////////////////////////////////////////////////////////////////// + + // Constructor. + // The first argument gives an estimate of the maximum number of nodes that can be added + // to the graph, and the second argument is an estimate of the maximum number of edges. + // The last (optional) argument is the pointer to the function which will be called + // if an error occurs; an error message is passed to this function. + // If this argument is omitted, exit(1) will be called. + // + // IMPORTANT: It is possible to add more nodes to the graph than node_num_max + // (and node_num_max can be zero). However, if the count is exceeded, then + // the internal memory is reallocated (increased by 50%) which is expensive. + // Also, temporarily the amount of allocated memory would be more than twice than needed. + // Similarly for edges. + // If you wish to avoid this overhead, you can download version 2.2, where nodes and edges are stored in blocks. + Graph(int node_num_max, int edge_num_max, void (*err_function)(char *) = NULL); + + // Destructor + ~Graph(); + + // Adds node(s) to the graph. By default, one node is added (num=1); then first call returns 0, second call returns 1, and so on. + // If num>1, then several nodes are added, and node_id of the first one is returned. + // IMPORTANT: see note about the constructor + node_id add_node(int num = 1); + + // Adds a bidirectional edge between 'i' and 'j' with the weights 'cap' and 'rev_cap'. + // IMPORTANT: see note about the constructor + void add_edge(node_id i, node_id j, captype cap, captype rev_cap); + + // Adds new edges 'SOURCE->i' and 'i->SINK' with corresponding weights. + // Can be called multiple times for each node. + // Weights can be negative. + // NOTE: the number of such edges is not counted in edge_num_max. + // No internal memory is allocated by this call. + void add_tweights(node_id i, tcaptype cap_source, tcaptype cap_sink); + + + // Computes the maxflow. Can be called several times. + // FOR DESCRIPTION OF reuse_trees, SEE mark_node(). + // FOR DESCRIPTION OF changed_list, SEE remove_from_changed_list(). + flowtype maxflow(bool reuse_trees = false, Block* changed_list = NULL); + + // After the maxflow is computed, this function returns to which + // segment the node 'i' belongs (Graph::SOURCE or Graph::SINK). + // + // Occasionally there may be several minimum cuts. If a node can be assigned + // to both the source and the sink, then default_segm is returned. + termtype what_segment(node_id i, termtype default_segm = SOURCE); + + + + ////////////////////////////////////////////// + // ADVANCED INTERFACE FUNCTIONS // + // (provide access to the graph) // + ////////////////////////////////////////////// + +private: + struct node; + struct arc; + +public: + + //////////////////////////// + // 1. Reallocating graph. // + //////////////////////////// + + // Removes all nodes and edges. + // After that functions add_node() and add_edge() must be called again. + // + // Advantage compared to deleting Graph and allocating it again: + // no calls to delete/new (which could be quite slow). + // + // If the graph structure stays the same, then an alternative + // is to go through all nodes/edges and set new residual capacities + // (see functions below). + void reset(); + + //////////////////////////////////////////////////////////////////////////////// + // 2. Functions for getting pointers to arcs and for reading graph structure. // + // NOTE: adding new arcs may invalidate these pointers (if reallocation // + // happens). So it's best not to add arcs while reading graph structure. // + //////////////////////////////////////////////////////////////////////////////// + + // The following two functions return arcs in the same order that they + // were added to the graph. NOTE: for each call add_edge(i,j,cap,cap_rev) + // the first arc returned will be i->j, and the second j->i. + // If there are no more arcs, then the function can still be called, but + // the returned arc_id is undetermined. + typedef arc* arc_id; + arc_id get_first_arc(); + arc_id get_next_arc(arc_id a); + + // other functions for reading graph structure + int get_node_num() { return node_num; } + int get_arc_num() { return (int)(arc_last - arcs); } + void get_arc_ends(arc_id a, node_id& i, node_id& j); // returns i,j to that a = i->j + + /////////////////////////////////////////////////// + // 3. Functions for reading residual capacities. // + /////////////////////////////////////////////////// + + // returns residual capacity of SOURCE->i minus residual capacity of i->SINK + tcaptype get_trcap(node_id i); + // returns residual capacity of arc a + captype get_rcap(arc* a); + + ///////////////////////////////////////////////////////////////// + // 4. Functions for setting residual capacities. // + // NOTE: If these functions are used, the value of the flow // + // returned by maxflow() will not be valid! // + ///////////////////////////////////////////////////////////////// + + void set_trcap(node_id i, tcaptype trcap); + void set_rcap(arc* a, captype rcap); + + //////////////////////////////////////////////////////////////////// + // 5. Functions related to reusing trees & list of changed nodes. // + //////////////////////////////////////////////////////////////////// + + // If flag reuse_trees is true while calling maxflow(), then search trees + // are reused from previous maxflow computation. + // In this case before calling maxflow() the user must + // specify which parts of the graph have changed by calling mark_node(): + // add_tweights(i),set_trcap(i) => call mark_node(i) + // add_edge(i,j),set_rcap(a) => call mark_node(i); mark_node(j) + // + // This option makes sense only if a small part of the graph is changed. + // The initialization procedure goes only through marked nodes then. + // + // mark_node(i) can either be called before or after graph modification. + // Can be called more than once per node, but calls after the first one + // do not have any effect. + // + // NOTE: + // - This option cannot be used in the first call to maxflow(). + // - It is not necessary to call mark_node() if the change is ``not essential'', + // i.e. sign(trcap) is preserved for a node and zero/nonzero status is preserved for an arc. + // - To check that you marked all necessary nodes, you can call maxflow(false) after calling maxflow(true). + // If everything is correct, the two calls must return the same value of flow. (Useful for debugging). + void mark_node(node_id i); + + // If changed_list is not NULL while calling maxflow(), then the algorithm + // keeps a list of nodes which could potentially have changed their segmentation label. + // Nodes which are not in the list are guaranteed to keep their old segmentation label (SOURCE or SINK). + // Example usage: + // + // typedef Graph G; + // G* g = new Graph(nodeNum, edgeNum); + // Block* changed_list = new Block(128); + // + // ... // add nodes and edges + // + // g->maxflow(); // first call should be without arguments + // for (int iter=0; iter<10; iter++) + // { + // ... // change graph, call mark_node() accordingly + // + // g->maxflow(true, changed_list); + // G::node_id* ptr; + // for (ptr=changed_list->ScanFirst(); ptr; ptr=changed_list->ScanNext()) + // { + // G::node_id i = *ptr; assert(i>=0 && iremove_from_changed_list(i); + // // do something with node i... + // if (g->what_segment(i) == G::SOURCE) { ... } + // } + // changed_list->Reset(); + // } + // delete changed_list; + // + // NOTE: + // - If changed_list option is used, then reuse_trees must be used as well. + // - In the example above, the user may omit calls g->remove_from_changed_list(i) and changed_list->Reset() in a given iteration. + // Then during the next call to maxflow(true, &changed_list) new nodes will be added to changed_list. + // - If the next call to maxflow() does not use option reuse_trees, then calling remove_from_changed_list() + // is not necessary. ("changed_list->Reset()" or "delete changed_list" should still be called, though). + void remove_from_changed_list(node_id i) + { + assert(i>=0 && i 0 then tr_cap is residual capacity of the arc SOURCE->node + // otherwise -tr_cap is residual capacity of the arc node->SINK + + }; + + struct arc + { + node *head; // node the arc points to + arc *next; // next arc with the same originating node + arc *sister; // reverse arc + + captype r_cap; // residual capacity + }; + + struct nodeptr + { + node *ptr; + nodeptr *next; + }; + static const int NODEPTR_BLOCK_SIZE = 128; + + node *nodes, *node_last, *node_max; // node_last = nodes+node_num, node_max = nodes+node_num_max; + arc *arcs, *arc_last, *arc_max; // arc_last = arcs+2*edge_num, arc_max = arcs+2*edge_num_max; + + int node_num; + + DBlock *nodeptr_block; + + void (*error_function)(char *); // this function is called if a error occurs, + // with a corresponding error message + // (or exit(1) is called if it's NULL) + + flowtype flow; // total flow + + // reusing trees & list of changed pixels + int maxflow_iteration; // counter + Block *changed_list; + + ///////////////////////////////////////////////////////////////////////// + + node *queue_first[2], *queue_last[2]; // list of active nodes + nodeptr *orphan_first, *orphan_last; // list of pointers to orphans + int TIME; // monotonically increasing global counter + + ///////////////////////////////////////////////////////////////////////// + + void reallocate_nodes(int num); // num is the number of new nodes + void reallocate_arcs(); + + // functions for processing active list + void set_active(node *i); + node *next_active(); + + // functions for processing orphans list + void set_orphan_front(node* i); // add to the beginning of the list + void set_orphan_rear(node* i); // add to the end of the list + + void add_to_changed_list(node* i); + + void maxflow_init(); // called if reuse_trees == false + void maxflow_reuse_trees_init(); // called if reuse_trees == true + void augment(arc *middle_arc); + void process_source_orphan(node *i); + void process_sink_orphan(node *i); + + void test_consistency(node* current_node=NULL); // debug function +}; + + + + + + + + + + + +/////////////////////////////////////// +// Implementation - inline functions // +/////////////////////////////////////// + + + +template + inline typename Graph::node_id Graph::add_node(int num) +{ + assert(num > 0); + + if (node_last + num > node_max) reallocate_nodes(num); + + if (num == 1) + { + node_last -> first = NULL; + node_last -> tr_cap = 0; + node_last -> is_marked = 0; + node_last -> is_in_changed_list = 0; + + node_last ++; + return node_num ++; + } + else + { + memset(node_last, 0, num*sizeof(node)); + + node_id i = node_num; + node_num += num; + node_last += num; + return i; + } +} + +template + inline void Graph::add_tweights(node_id i, tcaptype cap_source, tcaptype cap_sink) +{ + assert(i >= 0 && i < node_num); + + tcaptype delta = nodes[i].tr_cap; + if (delta > 0) cap_source += delta; + else cap_sink -= delta; + flow += (cap_source < cap_sink) ? cap_source : cap_sink; + nodes[i].tr_cap = cap_source - cap_sink; +} + +template + inline void Graph::add_edge(node_id _i, node_id _j, captype cap, captype rev_cap) +{ + assert(_i >= 0 && _i < node_num); + assert(_j >= 0 && _j < node_num); + assert(_i != _j); + assert(cap >= 0); + assert(rev_cap >= 0); + + if (arc_last == arc_max) reallocate_arcs(); + + arc *a = arc_last ++; + arc *a_rev = arc_last ++; + + node* i = nodes + _i; + node* j = nodes + _j; + + a -> sister = a_rev; + a_rev -> sister = a; + a -> next = i -> first; + i -> first = a; + a_rev -> next = j -> first; + j -> first = a_rev; + a -> head = j; + a_rev -> head = i; + a -> r_cap = cap; + a_rev -> r_cap = rev_cap; +} + +template + inline typename Graph::arc* Graph::get_first_arc() +{ + return arcs; +} + +template + inline typename Graph::arc* Graph::get_next_arc(arc* a) +{ + return a + 1; +} + +template + inline void Graph::get_arc_ends(arc* a, node_id& i, node_id& j) +{ + assert(a >= arcs && a < arc_last); + i = (node_id) (a->sister->head - nodes); + j = (node_id) (a->head - nodes); +} + +template + inline tcaptype Graph::get_trcap(node_id i) +{ + assert(i>=0 && i + inline captype Graph::get_rcap(arc* a) +{ + assert(a >= arcs && a < arc_last); + return a->r_cap; +} + +template + inline void Graph::set_trcap(node_id i, tcaptype trcap) +{ + assert(i>=0 && i + inline void Graph::set_rcap(arc* a, captype rcap) +{ + assert(a >= arcs && a < arc_last); + a->r_cap = rcap; +} + + +template + inline typename Graph::termtype Graph::what_segment(node_id i, termtype default_segm) +{ + if (nodes[i].parent) + { + return (nodes[i].is_sink) ? SINK : SOURCE; + } + else + { + return default_segm; + } +} + +template + inline void Graph::mark_node(node_id _i) +{ + node* i = nodes + _i; + if (!i->next) + { + /* it's not in the list yet */ + if (queue_last[1]) queue_last[1] -> next = i; + else queue_first[1] = i; + queue_last[1] = i; + i -> next = i; + } + i->is_marked = 1; +} + + +#endif diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/instances.inc b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/instances.inc new file mode 100644 index 00000000..1ea80f55 --- /dev/null +++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/instances.inc @@ -0,0 +1,16 @@ +#include "graph.h" + +#ifdef _MSC_VER +#pragma warning(disable: 4661) +#endif + +// Instantiations: +// IMPORTANT: +// flowtype should be 'larger' than tcaptype +// tcaptype should be 'larger' than captype + +//template class Graph; +//template class Graph; +//template class Graph; +//template class Graph; + diff --git a/quasimodo/quasimodo_models/src/modelupdater/graphcuts/maxflow.cpp b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/maxflow.cpp new file mode 100644 index 00000000..a2fc042b --- /dev/null +++ b/quasimodo/quasimodo_models/src/modelupdater/graphcuts/maxflow.cpp @@ -0,0 +1,684 @@ +/* maxflow.cpp */ + + +#include +#include "graph.h" + + +/* + special constants for node->parent +*/ +#define TERMINAL ( (arc *) 1 ) /* to terminal */ +#define ORPHAN ( (arc *) 2 ) /* orphan */ + + +#define INFINITE_D ((int)(((unsigned)-1)/2)) /* infinite distance to the terminal */ + +/***********************************************************************/ + +/* + Functions for processing active list. + i->next points to the next node in the list + (or to i, if i is the last node in the list). + If i->next is NULL iff i is not in the list. + + There are two queues. Active nodes are added + to the end of the second queue and read from + the front of the first queue. If the first queue + is empty, it is replaced by the second queue + (and the second queue becomes empty). +*/ + + +template + inline void Graph::set_active(node *i) +{ + if (!i->next) + { + /* it's not in the list yet */ + if (queue_last[1]) queue_last[1] -> next = i; + else queue_first[1] = i; + queue_last[1] = i; + i -> next = i; + } +} + +/* + Returns the next active node. + If it is connected to the sink, it stays in the list, + otherwise it is removed from the list +*/ +template + inline typename Graph::node* Graph::next_active() +{ + node *i; + + while ( 1 ) + { + if (!(i=queue_first[0])) + { + queue_first[0] = i = queue_first[1]; + queue_last[0] = queue_last[1]; + queue_first[1] = NULL; + queue_last[1] = NULL; + if (!i) return NULL; + } + + /* remove it from the active list */ + if (i->next == i) queue_first[0] = queue_last[0] = NULL; + else queue_first[0] = i -> next; + i -> next = NULL; + + /* a node in the list is active iff it has a parent */ + if (i->parent) return i; + } +} + +/***********************************************************************/ + +template + inline void Graph::set_orphan_front(node *i) +{ + nodeptr *np; + i -> parent = ORPHAN; + np = nodeptr_block -> New(); + np -> ptr = i; + np -> next = orphan_first; + orphan_first = np; +} + +template + inline void Graph::set_orphan_rear(node *i) +{ + nodeptr *np; + i -> parent = ORPHAN; + np = nodeptr_block -> New(); + np -> ptr = i; + if (orphan_last) orphan_last -> next = np; + else orphan_first = np; + orphan_last = np; + np -> next = NULL; +} + +/***********************************************************************/ + +template + inline void Graph::add_to_changed_list(node *i) +{ + if (changed_list && !i->is_in_changed_list) + { + node_id* ptr = changed_list->New(); + *ptr = (node_id)(i - nodes); + i->is_in_changed_list = true; + } +} + +/***********************************************************************/ + +template + void Graph::maxflow_init() +{ + node *i; + + queue_first[0] = queue_last[0] = NULL; + queue_first[1] = queue_last[1] = NULL; + orphan_first = NULL; + + TIME = 0; + + for (i=nodes; i next = NULL; + i -> is_marked = 0; + i -> is_in_changed_list = 0; + i -> TS = TIME; + if (i->tr_cap > 0) + { + /* i is connected to the source */ + i -> is_sink = 0; + i -> parent = TERMINAL; + set_active(i); + i -> DIST = 1; + } + else if (i->tr_cap < 0) + { + /* i is connected to the sink */ + i -> is_sink = 1; + i -> parent = TERMINAL; + set_active(i); + i -> DIST = 1; + } + else + { + i -> parent = NULL; + } + } +} + +template + void Graph::maxflow_reuse_trees_init() +{ + node* i; + node* j; + node* queue = queue_first[1]; + arc* a; + nodeptr* np; + + queue_first[0] = queue_last[0] = NULL; + queue_first[1] = queue_last[1] = NULL; + orphan_first = orphan_last = NULL; + + TIME ++; + + while ((i=queue)) + { + queue = i->next; + if (queue == i) queue = NULL; + i->next = NULL; + i->is_marked = 0; + set_active(i); + + if (i->tr_cap == 0) + { + if (i->parent) set_orphan_rear(i); + continue; + } + + if (i->tr_cap > 0) + { + if (!i->parent || i->is_sink) + { + i->is_sink = 0; + for (a=i->first; a; a=a->next) + { + j = a->head; + if (!j->is_marked) + { + if (j->parent == a->sister) set_orphan_rear(j); + if (j->parent && j->is_sink && a->r_cap > 0) set_active(j); + } + } + add_to_changed_list(i); + } + } + else + { + if (!i->parent || !i->is_sink) + { + i->is_sink = 1; + for (a=i->first; a; a=a->next) + { + j = a->head; + if (!j->is_marked) + { + if (j->parent == a->sister) set_orphan_rear(j); + if (j->parent && !j->is_sink && a->sister->r_cap > 0) set_active(j); + } + } + add_to_changed_list(i); + } + } + i->parent = TERMINAL; + i -> TS = TIME; + i -> DIST = 1; + } + + //test_consistency(); + + /* adoption */ + while ((np=orphan_first)) + { + orphan_first = np -> next; + i = np -> ptr; + nodeptr_block -> Delete(np); + if (!orphan_first) orphan_last = NULL; + if (i->is_sink) process_sink_orphan(i); + else process_source_orphan(i); + } + /* adoption end */ + + //test_consistency(); +} + +template + void Graph::augment(arc *middle_arc) +{ + node *i; + arc *a; + tcaptype bottleneck; + + + /* 1. Finding bottleneck capacity */ + /* 1a - the source tree */ + bottleneck = middle_arc -> r_cap; + for (i=middle_arc->sister->head; ; i=a->head) + { + a = i -> parent; + if (a == TERMINAL) break; + if (bottleneck > a->sister->r_cap) bottleneck = a -> sister -> r_cap; + } + if (bottleneck > i->tr_cap) bottleneck = i -> tr_cap; + /* 1b - the sink tree */ + for (i=middle_arc->head; ; i=a->head) + { + a = i -> parent; + if (a == TERMINAL) break; + if (bottleneck > a->r_cap) bottleneck = a -> r_cap; + } + if (bottleneck > - i->tr_cap) bottleneck = - i -> tr_cap; + + + /* 2. Augmenting */ + /* 2a - the source tree */ + middle_arc -> sister -> r_cap += bottleneck; + middle_arc -> r_cap -= bottleneck; + for (i=middle_arc->sister->head; ; i=a->head) + { + a = i -> parent; + if (a == TERMINAL) break; + a -> r_cap += bottleneck; + a -> sister -> r_cap -= bottleneck; + if (!a->sister->r_cap) + { + set_orphan_front(i); // add i to the beginning of the adoption list + } + } + i -> tr_cap -= bottleneck; + if (!i->tr_cap) + { + set_orphan_front(i); // add i to the beginning of the adoption list + } + /* 2b - the sink tree */ + for (i=middle_arc->head; ; i=a->head) + { + a = i -> parent; + if (a == TERMINAL) break; + a -> sister -> r_cap += bottleneck; + a -> r_cap -= bottleneck; + if (!a->r_cap) + { + set_orphan_front(i); // add i to the beginning of the adoption list + } + } + i -> tr_cap += bottleneck; + if (!i->tr_cap) + { + set_orphan_front(i); // add i to the beginning of the adoption list + } + + + flow += bottleneck; +} + +/***********************************************************************/ + +template + void Graph::process_source_orphan(node *i) +{ + node *j; + arc *a0, *a0_min = NULL, *a; + int d, d_min = INFINITE_D; + + /* trying to find a new parent */ + for (a0=i->first; a0; a0=a0->next) + if (a0->sister->r_cap) + { + j = a0 -> head; + if (!j->is_sink && (a=j->parent)) + { + /* checking the origin of j */ + d = 0; + while ( 1 ) + { + if (j->TS == TIME) + { + d += j -> DIST; + break; + } + a = j -> parent; + d ++; + if (a==TERMINAL) + { + j -> TS = TIME; + j -> DIST = 1; + break; + } + if (a==ORPHAN) { d = INFINITE_D; break; } + j = a -> head; + } + if (dhead; j->TS!=TIME; j=j->parent->head) + { + j -> TS = TIME; + j -> DIST = d --; + } + } + } + } + + if (i->parent = a0_min) + { + i -> TS = TIME; + i -> DIST = d_min + 1; + } + else + { + /* no parent is found */ + add_to_changed_list(i); + + /* process neighbors */ + for (a0=i->first; a0; a0=a0->next) + { + j = a0 -> head; + if (!j->is_sink && (a=j->parent)) + { + if (a0->sister->r_cap) set_active(j); + if (a!=TERMINAL && a!=ORPHAN && a->head==i) + { + set_orphan_rear(j); // add j to the end of the adoption list + } + } + } + } +} + +template + void Graph::process_sink_orphan(node *i) +{ + node *j; + arc *a0, *a0_min = NULL, *a; + int d, d_min = INFINITE_D; + + /* trying to find a new parent */ + for (a0=i->first; a0; a0=a0->next) + if (a0->r_cap) + { + j = a0 -> head; + if (j->is_sink && (a=j->parent)) + { + /* checking the origin of j */ + d = 0; + while ( 1 ) + { + if (j->TS == TIME) + { + d += j -> DIST; + break; + } + a = j -> parent; + d ++; + if (a==TERMINAL) + { + j -> TS = TIME; + j -> DIST = 1; + break; + } + if (a==ORPHAN) { d = INFINITE_D; break; } + j = a -> head; + } + if (dhead; j->TS!=TIME; j=j->parent->head) + { + j -> TS = TIME; + j -> DIST = d --; + } + } + } + } + + if (i->parent = a0_min) + { + i -> TS = TIME; + i -> DIST = d_min + 1; + } + else + { + /* no parent is found */ + add_to_changed_list(i); + + /* process neighbors */ + for (a0=i->first; a0; a0=a0->next) + { + j = a0 -> head; + if (j->is_sink && (a=j->parent)) + { + if (a0->r_cap) set_active(j); + if (a!=TERMINAL && a!=ORPHAN && a->head==i) + { + set_orphan_rear(j); // add j to the end of the adoption list + } + } + } + } +} + +/***********************************************************************/ + +template + flowtype Graph::maxflow(bool reuse_trees, Block* _changed_list) +{ + node *i, *j, *current_node = NULL; + arc *a; + nodeptr *np, *np_next; + + if (!nodeptr_block) + { + nodeptr_block = new DBlock(NODEPTR_BLOCK_SIZE, error_function); + } + + changed_list = _changed_list; + if (maxflow_iteration == 0 && reuse_trees) { if (error_function) (*error_function)("reuse_trees cannot be used in the first call to maxflow()!"); exit(1); } + if (changed_list && !reuse_trees) { if (error_function) (*error_function)("changed_list cannot be used without reuse_trees!"); exit(1); } + + if (reuse_trees) maxflow_reuse_trees_init(); + else maxflow_init(); + + // main loop + while ( 1 ) + { + // test_consistency(current_node); + + if ((i=current_node)) + { + i -> next = NULL; /* remove active flag */ + if (!i->parent) i = NULL; + } + if (!i) + { + if (!(i = next_active())) break; + } + + /* growth */ + if (!i->is_sink) + { + /* grow source tree */ + for (a=i->first; a; a=a->next) + if (a->r_cap) + { + j = a -> head; + if (!j->parent) + { + j -> is_sink = 0; + j -> parent = a -> sister; + j -> TS = i -> TS; + j -> DIST = i -> DIST + 1; + set_active(j); + add_to_changed_list(j); + } + else if (j->is_sink) break; + else if (j->TS <= i->TS && + j->DIST > i->DIST) + { + /* heuristic - trying to make the distance from j to the source shorter */ + j -> parent = a -> sister; + j -> TS = i -> TS; + j -> DIST = i -> DIST + 1; + } + } + } + else + { + /* grow sink tree */ + for (a=i->first; a; a=a->next) + if (a->sister->r_cap) + { + j = a -> head; + if (!j->parent) + { + j -> is_sink = 1; + j -> parent = a -> sister; + j -> TS = i -> TS; + j -> DIST = i -> DIST + 1; + set_active(j); + add_to_changed_list(j); + } + else if (!j->is_sink) { a = a -> sister; break; } + else if (j->TS <= i->TS && + j->DIST > i->DIST) + { + /* heuristic - trying to make the distance from j to the sink shorter */ + j -> parent = a -> sister; + j -> TS = i -> TS; + j -> DIST = i -> DIST + 1; + } + } + } + + TIME ++; + + if (a) + { + i -> next = i; /* set active flag */ + current_node = i; + + /* augmentation */ + augment(a); + /* augmentation end */ + + /* adoption */ + while ((np=orphan_first)) + { + np_next = np -> next; + np -> next = NULL; + + while ((np=orphan_first)) + { + orphan_first = np -> next; + i = np -> ptr; + nodeptr_block -> Delete(np); + if (!orphan_first) orphan_last = NULL; + if (i->is_sink) process_sink_orphan(i); + else process_source_orphan(i); + } + + orphan_first = np_next; + } + /* adoption end */ + } + else current_node = NULL; + } + // test_consistency(); + + if (!reuse_trees || (maxflow_iteration % 64) == 0) + { + delete nodeptr_block; + nodeptr_block = NULL; + } + + maxflow_iteration ++; + return flow; +} + +/***********************************************************************/ + + +template + void Graph::test_consistency(node* current_node) +{ + node *i; + arc *a; + int r; + int num1 = 0, num2 = 0; + + // test whether all nodes i with i->next!=NULL are indeed in the queue + for (i=nodes; inext || i==current_node) num1 ++; + } + for (r=0; r<3; r++) + { + i = (r == 2) ? current_node : queue_first[r]; + if (i) + for ( ; ; i=i->next) + { + num2 ++; + if (i->next == i) + { + if (r<2) assert(i == queue_last[r]); + else assert(i == current_node); + break; + } + } + } + assert(num1 == num2); + + for (i=nodes; iparent == NULL) {} + else if (i->parent == ORPHAN) {} + else if (i->parent == TERMINAL) + { + if (!i->is_sink) assert(i->tr_cap > 0); + else assert(i->tr_cap < 0); + } + else + { + if (!i->is_sink) assert (i->parent->sister->r_cap > 0); + else assert (i->parent->r_cap > 0); + } + // test whether passive nodes in search trees have neighbors in + // a different tree through non-saturated edges + if (i->parent && !i->next) + { + if (!i->is_sink) + { + assert(i->tr_cap >= 0); + for (a=i->first; a; a=a->next) + { + if (a->r_cap > 0) assert(a->head->parent && !a->head->is_sink); + } + } + else + { + assert(i->tr_cap <= 0); + for (a=i->first; a; a=a->next) + { + if (a->sister->r_cap > 0) assert(a->head->parent && a->head->is_sink); + } + } + } + // test marking invariants + if (i->parent && i->parent!=ORPHAN && i->parent!=TERMINAL) + { + assert(i->TS <= i->parent->head->TS); + if (i->TS == i->parent->head->TS) assert(i->DIST > i->parent->head->DIST); + } + } +} + +#include "instances.inc" diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 18b1a4a9..bf39bf18 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -104,6 +104,209 @@ MassRegistrationPPR2::~MassRegistrationPPR2(){ delete[] kp_rangeW_arr; } +void MassRegistrationPPR2::addModel(Model * model){ + + double total_load_time_start = getTime(); +// frames.push_back(frame); +// mmasks.push_back(mmask); +/* + nr_matches.push_back( 0); + matchids.push_back( std::vector< std::vector >() ); + matchdists.push_back( std::vector< std::vector >() ); + nr_datas.push_back( 0); + + points.push_back( Eigen::Matrix()); + colors.push_back( Eigen::Matrix()); + normals.push_back( Eigen::Matrix()); + transformed_points.push_back( Eigen::Matrix()); + transformed_normals.push_back( Eigen::Matrix()); + + informations.push_back( Eigen::VectorXd()); + + nr_arraypoints.push_back(0); + arraypoints.push_back(0); + arraynormals.push_back(0); + arraycolors.push_back(0); + arrayinformations.push_back(0); + trees3d.push_back(0); + a3dv.push_back(0); + + depthedge_nr_arraypoints.push_back(0); + depthedge_arraypoints.push_back(0); + depthedge_arrayinformations.push_back(0); + depthedge_trees3d.push_back(0); + depthedge_a3dv.push_back(0); + + + is_ok.push_back(false); + + unsigned int i = points.size()-1; + + bool * maskvec = mmask->maskvec; + unsigned char * edgedata = (unsigned char *)(frame->depthedges.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + int count = 0; + for(unsigned int w = 0; w < width; w+=maskstep){ + for(unsigned int h = 0; h < height; h+=maskstep){ + int ind = h*width+w; + if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ + float z = idepth*float(depthdata[ind]); + float xn = normalsdata[3*ind+0]; + if(z > 0.2 && xn != 2){count++;} + } + } + } + + + if(count < 10){ + is_ok[i] = false; + return; + }else{ + is_ok[i] = true; + } + +// printf("%i is ok %i\n",i,is_ok[i]); + + nr_datas[i] = count; + points[i].resize(Eigen::NoChange,count); + colors[i].resize(Eigen::NoChange,count); + normals[i].resize(Eigen::NoChange,count); + transformed_points[i].resize(Eigen::NoChange,count); + transformed_normals[i].resize(Eigen::NoChange,count); + + double * ap = new double[3*count]; + double * an = new double[3*count]; + double * ac = new double[3*count]; + double * ai = new double[3*count]; + nr_arraypoints[i] = count; + arraypoints[i] = ap; + arraynormals[i] = an; + arraycolors[i] = ac; + arrayinformations[i] = ai; + + Eigen::Matrix & X = points[i]; + Eigen::Matrix & C = colors[i]; + Eigen::Matrix & Xn = normals[i]; + Eigen::Matrix & tX = transformed_points[i]; + Eigen::Matrix & tXn = transformed_normals[i]; + Eigen::VectorXd information (count); + + int c = 0; + for(unsigned int w = 0; w < width; w+=maskstep){ + for(unsigned int h = 0; h < height;h+=maskstep){ + if(c == count){continue;} + int ind = h*width+w; + //if(maskvec[ind] || nomask){ + if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ + float z = idepth*float(depthdata[ind]); + float xn = normalsdata[3*ind+0]; + + if(z > 0.2 && xn != 2){ + float yn = normalsdata[3*ind+1]; + float zn = normalsdata[3*ind+2]; + + float x = (w - cx) * z * ifx; + float y = (h - cy) * z * ify; + + ap[3*c+0] =x; + ap[3*c+1] =y; + ap[3*c+2] =z; + + an[3*c+0] =xn; + an[3*c+1] =yn; + an[3*c+2] =zn; + + ac[3*c+0] =rgbdata[3*ind+2]; + ac[3*c+1] =rgbdata[3*ind+1]; + ac[3*c+2] =rgbdata[3*ind+0]; + + ai[c] = pow(fabs(z),-2);//1.0/(z*z); + + X(0,c) = x; + X(1,c) = y; + X(2,c) = z; + Xn(0,c) = xn; + Xn(1,c) = yn; + Xn(2,c) = zn; + + information(c) = ai[c];//1.0/(z*z); + C(0,c) = rgbdata[3*ind+0]; + C(1,c) = rgbdata[3*ind+1]; + C(2,c) = rgbdata[3*ind+2]; + c++; + } + } + } + } + informations[i] = information; + + ArrayData3D * a3d = new ArrayData3D; + a3d->data = ap; + a3d->rows = count; + a3dv[i] = a3d; + trees3d[i] = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + trees3d[i]->buildIndex(); + + + int depthedge_count = 0; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height; h++){ + int ind = h*width+w; + if(maskvec[ind] && edgedata[ind] == 255){ + float z = idepth*float(depthdata[ind]); + if(z > 0.2){depthedge_count++;} + } + } + } + + if(depthedge_count < 10){ + double * depthedge_ap = new double[3*depthedge_count]; + double * depthedge_ai = new double[3*depthedge_count]; + depthedge_nr_arraypoints[i] = depthedge_count; + depthedge_arraypoints[i] = depthedge_ap; + depthedge_arrayinformations[i] = depthedge_ai; + + c = 0; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height; h++){ + if(c == depthedge_count){continue;} + int ind = h*width+w; + if(maskvec[ind] && edgedata[ind] == 255){ + float z = idepth*float(depthdata[ind]); + if(z > 0.2){ + depthedge_ap[3*c+0] = (w - cx) * z * ifx; + depthedge_ap[3*c+1] = (h - cy) * z * ify;; + depthedge_ap[3*c+2] = z; + depthedge_ai[c] = pow(fabs(z),-2);//1.0/(z*z); + c++; + } + } + } + } + + ArrayData3D * depthedge_a3d = new ArrayData3D; + depthedge_a3d->data = depthedge_ap; + depthedge_a3d->rows = depthedge_count; + depthedge_a3dv[i] = depthedge_a3d; + depthedge_trees3d[i] = new Tree3d(3, *depthedge_a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + depthedge_trees3d[i]->buildIndex(); + } +*/ + printf("total load time: %5.5f\n",getTime()-total_load_time_start); +} + void MassRegistrationPPR2::addModelData(Model * model_, bool submodels){ model = model_; printf("addModelData\n"); diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp index 41179ca6..ebbb40de 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp @@ -188,6 +188,77 @@ double fitCurrentHist(std::vector & A, std::vector & B){ return pbest; } +void blurHistogram3(std::vector & blur_hist, std::vector & hist, float stdval, int histogramsize, bool debugg_print = false){ + int nr_data = histogramsize;//blur_hist.size(); + +//if(debugg_print){ +// for(int i = 0; i < nr_data; i++){ +// printf("%i -> %f\n",i,hist[i]); +// } +//} + + int nr_data2 = 2*nr_data; + + std::vector tmphist; + tmphist.resize(nr_data2); + + std::vector tmphist_blur; + tmphist_blur.resize(nr_data2); + + for(int i = 0; i < nr_data; i++){ + tmphist[i] = hist[nr_data-1-i]; + tmphist[nr_data+i] = hist[i]; + tmphist_blur[i] = 0; + tmphist_blur[nr_data+i] = 0; + } + + double info = -0.5/(stdval*stdval); + double weights[nr_data2]; + for(int i = 0; i < nr_data2; i++){weights[i] = 0;} + for(int i = 0; i < nr_data2; i++){ + double current = exp(i*i*info); + weights[i] = current; + if(current < 0.001){break;} + } + + int offset = 4.0*stdval; + offset = std::max(4,offset); + for(int i = 0; i < nr_data2; i++){ + double v = tmphist[i]; + if(v == 0){continue;} + + //if(debugg_print){printf("v: %5.5f\n",v);} + + int start = std::max(0,i-offset); + int stop = std::min(nr_data2,i+offset+1); + + for(int j = start; j < stop; j++){tmphist_blur[j] += v*weights[abs(i-j)];} + //if(debugg_print){exit(0);} + } + for(int i = 0; i < nr_data; i++){blur_hist[i] = tmphist_blur[i+nr_data];} + + float bef = 0; + float aft = 0; + for(int i = 0; i < nr_data; i++){ + bef += hist[i]; + aft += blur_hist[i]; + } + + for(int i = 0; i < nr_data; i++){blur_hist[i] *= bef/aft;} + +// printf("hist = ["); for(int k = 0; k < 300 && k < hist.size(); k++){printf("%i ",int(hist[k]));} printf("];\n"); +// printf("hist_smooth = ["); for(int k = 0; k < 300 && k < blur_hist.size(); k++){printf("%i ",int(blur_hist[k]));} printf("];\n"); +// double p = fitCurrentHist(hist, blur_hist); +// printf("p = %f;\n",p); +// double p2 = fitCurrentHist2(hist, blur_hist); +// printf("p2 = %f;\n",p2); +// for(double p = 1.0; p < 3; p+=0.001){ +// printf("%3.3f -> %8.8f\n",p,scoreCurrentHist(p, hist, blur_hist)); +// } +// exit(0); +} + + void blurHistogram2(std::vector & blur_hist, std::vector & hist, float stdval,bool debugg_print = false){ int nr_data = blur_hist.size(); int nr_data2 = 2*nr_data; @@ -427,6 +498,9 @@ void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ const unsigned int nr_data = mat.cols(); const int nr_dim = mat.rows(); + + //if(debugg_print){printf("histogram_size: %i maxd: %f\n",histogram_size,maxd);exit(0);} +if(debugg_print){printf("\n################################################### ITER:%i ############################################################\n",iter);} if(!fixed_histogram_size){ if(first){ first = false; @@ -439,7 +513,6 @@ if(!fixed_histogram_size){ } if(update_size){ - if(debugg_print){printf("\n################################################### ITER:%i ############################################################\n",iter);} maxd = fabs(meanval2) + (stdval2 + regularization)*target_length; if(debugg_print){printf("maxd: %f\n",maxd);} } @@ -467,25 +540,47 @@ if(!fixed_histogram_size){ double start_time = getCurrentTime3(); for(int j = 0; j < histogram_size; j++){histogram[j] = 0;} +// for(unsigned int j = 0; j < nr_data; j++){ +// for(int k = 0; k < nr_dim; k++){ +// double ind = fabs(mat(k,j))*histogram_mul; +// double w2 = ind-int(ind); +// double w1 = 1-w2; +// if(ind >= 0 && (ind+0.5) < histogram_size){ +// histogram[int(ind+0.5)]++; +// } +// if(j % 5000 == 0){printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind);} +// } +// } + +if(!fixed_histogram_size){ for(unsigned int j = 0; j < nr_data; j++){ for(int k = 0; k < nr_dim; k++){ - double ind = fabs(mat(k,j))*histogram_mul; double w2 = ind-int(ind); double w1 = 1-w2; if(ind >= 0 && (ind+0.5) < histogram_size){ histogram[int(ind+0.5)]++; } - - //printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind); + //if(j % 5000 == 0){printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind);} } } - -if(!fixed_histogram_size){ histogram[0]*=2; +}else{ + for(unsigned int j = 0; j < nr_data; j++){ + for(int k = 0; k < nr_dim; k++){ + double ind = fabs(mat(k,j))*histogram_mul; + double w2 = ind-int(ind); + double w1 = 1-w2; + if(ind >= 0 && (ind+0.00001) < histogram_size){ + histogram[int(ind+0.00001)]++; + } + //if(j % 5000 == 0){printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind);} + } + } } start_time = getCurrentTime3(); - blurHistogram2(blur_histogram,histogram,blurval,false); + blurHistogram3(blur_histogram,histogram,blurval,histogram_size,debugg_print); + //blurHistogram2(blur_histogram,histogram,blurval,false); Gaussian3 g = getModel(stdval,blur_histogram,uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); @@ -689,6 +784,12 @@ void DistanceWeightFunction2PPR2::reset(){ meanval2 = 0; iter = 0; first = true; + +// if(debugg_print){ +// printf("starthistogram_size: %i startmaxd: %f\n",starthistogram_size,startmaxd); +// printf("histogram_size: %i maxd: %f\n",histogram_size,maxd); +// exit(0); +// } } std::string DistanceWeightFunction2PPR2::getString(){ diff --git a/quasimodo/quasimodo_msgs/CMakeLists.txt b/quasimodo/quasimodo_msgs/CMakeLists.txt index fc559d4e..e0e48f5d 100644 --- a/quasimodo/quasimodo_msgs/CMakeLists.txt +++ b/quasimodo/quasimodo_msgs/CMakeLists.txt @@ -64,6 +64,7 @@ add_service_files( query_cloud.srv simple_query_cloud.srv visualize_query.srv + segment_model.srv ) ## Generate actions in the 'action' folder diff --git a/quasimodo/quasimodo_msgs/srv/segment_model.srv b/quasimodo/quasimodo_msgs/srv/segment_model.srv new file mode 100644 index 00000000..097bffb1 --- /dev/null +++ b/quasimodo/quasimodo_msgs/srv/segment_model.srv @@ -0,0 +1,8 @@ +model backgroundmodel +model[] models +uint64 background_refine +uint64 background_update +uint64 models_refine +--- +model backgroundmodel +model[] models From 0e67f0e9633df017ceb09843c3b1b0d07d93a470 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Sun, 17 Jul 2016 19:23:50 +0200 Subject: [PATCH 037/255] added better support for frame to model registration --- .../src/segmentationserver.cpp | 167 +++++++++--------- .../quasimodo_brain/src/test_segment.cpp | 27 ++- .../src/registration/MassRegistration.cpp | 87 +++++---- .../src/registration/MassRegistrationPPR2.cpp | 101 +++-------- 4 files changed, 184 insertions(+), 198 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index 134757f4..1209462b 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -39,12 +39,46 @@ boost::shared_ptr viewer; bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs::segment_model::Response & res){ printf("segment_model\n"); - reglib::Model * bg = quasimodo_brain::getModelFromMSG(req.backgroundmodel); std::vector< reglib::Model * > models; for(unsigned int i = 0; i < req.models.size(); i++){ models.push_back(quasimodo_brain::getModelFromMSG(req.models[i])); } + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + reglib::Model * bg = quasimodo_brain::getModelFromMSG(req.backgroundmodel); + + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.0); + bgmassreg->timeout = 1200; + bgmassreg->viewer = viewer; + bgmassreg->visualizationLvl = 0; + bgmassreg->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); + bgmassreg->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->setData(bg->frames,bg->modelmasks); + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(bg->relativeposes); + bg->relativeposes = bgmfr.poses; + + + bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); +// printf("frames: %i\n",bg->frames.size()); +// for(int i = 0; i < bg->frames.size(); i++){ +// cv::imshow("modelmask", bg->modelmasks[i]->getMask()); +// bg->frames[i]->show(true); +// } +// printf("points: %i\n",bg->points.size()); + +// viewer->removeAllPointClouds(); +// pcl::PointCloud::Ptr cloud = bg->getPCLcloud(1, false); +// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); +// viewer->spin(); +// printf("cloudsize: %i\n",cloud->points.size()); + vector cp; vector cf; vector mm; @@ -52,18 +86,33 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs vector cp_front; vector cf_front; vector mm_front; - for(int j = 0; j < models.size(); j++){ - cp_front.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); - cf_front.push_back(models[j]->frames.front()); - mm_front.push_back(models[j]->modelmasks.front()); + + + if(bg->frames.size() > 0){ + cp_front.push_back(Eigen::Matrix4d::Identity()); + cf_front.push_back(bg->frames.front()); + mm_front.push_back(bg->modelmasks.front()); + + for(int j = 0; j < models.size(); j++){ + cp_front.push_back(bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); + cf_front.push_back(models[j]->frames.front()); + mm_front.push_back(models[j]->modelmasks.front()); + } + }else{ + for(int j = 0; j < models.size(); j++){ + cp_front.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); + cf_front.push_back(models[j]->frames.front()); + mm_front.push_back(models[j]->modelmasks.front()); + } } + printf("cp_front: %i\n",cp_front.size()); - if(models.size() > 2){ + if(models.size() > 2 || (models.size() > 1 && bg->frames.size() > 0)){ reglib::MassRegistrationPPR2 * massreg = new reglib::MassRegistrationPPR2(0.05); massreg->timeout = 1200; massreg->viewer = viewer; - massreg->visualizationLvl = 0; + massreg->visualizationLvl = 1; massreg->maskstep = 5;//std::max(1,int(0.4*double(models[i]->frames.size()))); massreg->nomaskstep = 5;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); @@ -94,7 +143,6 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs } } - printf("cp: %i\n",cp.size()); reglib::MassRegistrationPPR2 * massreg2 = new reglib::MassRegistrationPPR2(0.0); @@ -108,14 +156,21 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs massreg2->stopval = 0.0005; massreg2->setData(cf,mm); + + if(bg->frames.size() > 0){ + massreg2->addModel(bg); + cp.push_back(Eigen::Matrix4d::Identity()); + printf("---->added background model\n"); + } + reglib::MassFusionResults mfr2 = massreg2->getTransforms(cp); cp = mfr2.poses; - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; + Eigen::Matrix4d relative_to_bg = Eigen::Matrix4d::Identity(); + if(bg->frames.size() > 0){ + relative_to_bg = cp.front().inverse()*cp.back(); + cp.pop_back(); + } vector masks; for(unsigned int i = 0; i < cf.size(); i++){ @@ -126,24 +181,24 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs masks.push_back(mask); } std::vector internal_masks = mu->computeDynamicObject(0,cp,cf,masks);//Determine self occlusions - std::vector dynamic_masks = mu->computeDynamicObject(bg,cp,cf,internal_masks);//Determine occlusion of background occlusions - //add new frames to background - //Compute minimum required frames to capture background - for(unsigned int i = 0; i < cf.size(); i++){ - - cv::Mat mask; - mask.create(cf[i]->camera->height,cf[i]->camera->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); - unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[i].data); - for(unsigned int i = 0; i < cf[i]->camera->height*cf[i]->camera->width;i++){ - maskdata[i] = std::max(internalmaskdata[i],dynamicmaskdata[i]); - } - - bg->frames.push_back(cf[i]); - bg->relativeposes.push_back(cp[i]); - bg->modelmasks.push_back(new reglib::ModelMask(mask)); - } +// std::vector dynamic_masks = mu->computeDynamicObject(bg,cp,cf,internal_masks);//Determine occlusion of background occlusions +// //add new frames to background +// //Compute minimum required frames to capture background +// for(unsigned int i = 0; i < cf.size(); i++){ + +// cv::Mat mask; +// mask.create(cf[i]->camera->height,cf[i]->camera->width,CV_8UC1); +// unsigned char * maskdata = (unsigned char *)(mask.data); +// unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); +// unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[i].data); +// for(unsigned int i = 0; i < cf[i]->camera->height*cf[i]->camera->width;i++){ +// maskdata[i] = std::max(internalmaskdata[i],dynamicmaskdata[i]); +// } + +// bg->frames.push_back(cf[i]); +// bg->relativeposes.push_back(cp[i]); +// bg->modelmasks.push_back(new reglib::ModelMask(mask)); +// } return true; } @@ -156,56 +211,6 @@ int main(int argc, char** argv){ for(int i = 1; i < argc;i++){ printf("input: %s\n",argv[i]); if(std::string(argv[i]).compare("-v") == 0){ printf("visualization turned on\n"); visualization = true;} - /* - if( std::string(argv[i]).compare("-c") == 0){ printf("camera input state\n"); inputstate = 1;} - else if(std::string(argv[i]).compare("-l") == 0){ printf("reading all models at %s (the path defined from -p)\n",savepath.c_str()); - std::vector folderdata; - int r = getdir(savepath+"/",folderdata); - for(unsigned int fnr = 0; fnr < folderdata.size(); fnr++){ - printf("%s\n",folderdata[fnr].c_str()); - } - exit(0);} - else if(std::string(argv[i]).compare("-m") == 0){ printf("model input state\n"); inputstate = 2;} - else if(std::string(argv[i]).compare("-p") == 0){ printf("path input state\n"); inputstate = 3;} - else if(std::string(argv[i]).compare("-occlusion_penalty") == 0){printf("occlusion_penalty input state\n");inputstate = 4;} - else if(std::string(argv[i]).compare("-massreg_timeout") == 0){printf("massreg_timeout input state\n");inputstate = 5;} - else if(std::string(argv[i]).compare("-search") == 0){printf("pointcloud search input state\n");run_search = true; inputstate = 6;} - else - else if(std::string(argv[i]).compare("-v_init") == 0){ printf("visualization of init turned on\n"); visualization = true; inputstate = 8;} - else if(std::string(argv[i]).compare("-v_refine") == 0 || std::string(argv[i]).compare("-v_ref") == 0){ printf("visualization refinement turned on\n"); visualization = true; inputstate = 9;} - else if(std::string(argv[i]).compare("-v_register") == 0 || std::string(argv[i]).compare("-v_reg") == 0){ printf("visualization registration turned on\n"); visualization = true; inputstate = 10;} - else if(std::string(argv[i]).compare("-v_scoring") == 0 || std::string(argv[i]).compare("-v_score") == 0 || std::string(argv[i]).compare("-v_sco") == 0){ printf("visualization scoring turned on\n"); visualization = true; show_scoring = true;} - else if(std::string(argv[i]).compare("-v_db") == 0){ printf("visualization db turned on\n"); visualization = true; show_db = true;} - else if(inputstate == 1){ - reglib::Camera * cam = reglib::Camera::load(std::string(argv[i])); - delete cameras[0]; - cameras[0] = cam; - }else if(inputstate == 2){ - reglib::Model * model = reglib::Model::load(cameras[0],std::string(argv[i])); - sweepid_counter = std::max(int(model->modelmasks[0]->sweepid + 1), sweepid_counter); - modeldatabase->add(model); - //addToDB(modeldatabase, model,false); - model->last_changed = ++current_model_update; - show_sorted(); - }else if(inputstate == 3){ - savepath = std::string(argv[i]); - }else if(inputstate == 4){ - occlusion_penalty = atof(argv[i]); printf("occlusion_penalty set to %f\n",occlusion_penalty); - }else if(inputstate == 5){ - massreg_timeout = atof(argv[i]); printf("massreg_timeout set to %f\n",massreg_timeout); - }else if(inputstate == 6){ - search_timeout = atof(argv[i]); printf("search_timeout set to %f\n",search_timeout); - if(search_timeout == 0){ - run_search = false; - } - }else if(inputstate == 8){ - show_init_lvl = atoi(argv[i]); - }else if(inputstate == 9){ - show_refine_lvl = atoi(argv[i]); - }else if(inputstate == 10){ - show_reg_lvl = atoi(argv[i]); - } - */ } if(visualization){ diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index cd129cee..d99a4933 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -329,11 +329,6 @@ reglib::Model * load2(std::string sweep_xml){ SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS -// cam->fx = 536.458000; -// cam->fy = 537.422000; -// cam->cx = 314.458000; -// cam->cy = 242.038000; - cam->fx = 532.158936; cam->fy = 533.819214; cam->cx = 310.514310; @@ -379,7 +374,7 @@ reglib::Model * load2(std::string sweep_xml){ cv::Mat fullmask; fullmask.create(480,640,CV_8UC1); unsigned char * maskdata = (unsigned char *)fullmask.data; - for(int j = 0; j < 480*640; j++){maskdata[j] = 0;} + for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} reglib::Model * sweepmodel = 0; std::vector current_room_frames; @@ -707,9 +702,27 @@ int main(int argc, char** argv){ } } - for(unsigned int i = 0; i < models.size(); i++){ + for(unsigned int i = 1; i < models.size(); i++){ quasimodo_msgs::segment_model sm; sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); + if(i > 0){ + +// reglib::MassRegistrationPPR2 * massreg2 = new reglib::MassRegistrationPPR2(0.0); +// massreg2->timeout = 1200; +// massreg2->viewer = viewer; +// massreg2->visualizationLvl = 0; + +// massreg2->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); +// massreg2->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); +// massreg2->nomask = true; +// massreg2->stopval = 0.0005; + +// massreg2->setData(models[i-1]->frames,models[i-1]->modelmasks); +// reglib::MassFusionResults mfr2 = massreg2->getTransforms(models[i-1]->relativeposes); +// models[i-1]->relativeposes = mfr2.poses; + + sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]); + } if (segmentation_client.call(sm)){//Build model from frame //int model_id = mff.response.model_id; printf("segmented: %i\n",i); diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp index 6810048e..cfc149e3 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp @@ -82,46 +82,59 @@ Eigen::MatrixXd MassRegistration::getMat(int rows, int cols, double * datas){ void MassRegistration::show(std::vector Xv, bool save, std::string filename, bool stop){ - viewer->removeAllPointClouds(); + //for(unsigned int a = 0; a < Xv.size(); a++){ + viewer->removeAllPointClouds(); + + srand(0); + for(unsigned int xi = 0; xi < Xv.size(); xi++){ + + Eigen::MatrixXd X = Xv[xi]; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + int r,g,b; +// if(xi == a){ +// r = 0; +// g = 255; +// b = 0; +// }else{ +// r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); +// g = 0;//255*((xi+1) & 1); +// b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1); +// } + + r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); + g = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); + b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1); + + unsigned int nr_data = X.cols(); + cloud->points.clear(); + for(unsigned int i = 0; i < nr_data; i++){ + pcl::PointXYZRGBNormal p; + p.x = X(0,i); + p.y = X(1,i); + p.z = X(2,i); + p.b = r; + p.g = g; + p.r = b; + cloud->points.push_back(p); + } + char buf [1024]; + sprintf(buf,"cloud%i",xi); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); - srand(0); - for(unsigned int xi = 0; xi < Xv.size(); xi++){ - - Eigen::MatrixXd X = Xv[xi]; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - int r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); - int g = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); - int b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1); - - unsigned int nr_data = X.cols(); - cloud->points.clear(); - for(unsigned int i = 0; i < nr_data; i++){ - pcl::PointXYZRGBNormal p; - p.x = X(0,i); - p.y = X(1,i); - p.z = X(2,i); - p.b = r; - p.g = g; - p.r = b; - cloud->points.push_back(p); } - char buf [1024]; - sprintf(buf,"cloud%i",xi); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); - - } - if(!save){ - viewer->spin(); - }else{ - viewer->spinOnce(); - } - //void pcl::visualization::PCLVisualizerInteractorStyle::saveScreenshot ( const std::string & file ) - if(save){ - printf("saving: %s\n",filename.c_str()); - viewer->saveScreenshot(filename); - } - viewer->removeAllPointClouds(); + if(!save){ + viewer->spin(); + }else{ + viewer->spinOnce(); + } + //void pcl::visualization::PCLVisualizerInteractorStyle::saveScreenshot ( const std::string & file ) + if(save){ + printf("saving: %s\n",filename.c_str()); + viewer->saveScreenshot(filename); + } + viewer->removeAllPointClouds(); + //} } /* void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){ diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index bf39bf18..2a6f9f59 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -107,9 +107,8 @@ MassRegistrationPPR2::~MassRegistrationPPR2(){ void MassRegistrationPPR2::addModel(Model * model){ double total_load_time_start = getTime(); -// frames.push_back(frame); -// mmasks.push_back(mmask); -/* + + nr_matches.push_back( 0); matchids.push_back( std::vector< std::vector >() ); matchdists.push_back( std::vector< std::vector >() ); @@ -137,39 +136,13 @@ void MassRegistrationPPR2::addModel(Model * model){ depthedge_trees3d.push_back(0); depthedge_a3dv.push_back(0); + int step = maskstep*maskstep; is_ok.push_back(false); - unsigned int i = points.size()-1; - - bool * maskvec = mmask->maskvec; - unsigned char * edgedata = (unsigned char *)(frame->depthedges.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); - - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - - int count = 0; - for(unsigned int w = 0; w < width; w+=maskstep){ - for(unsigned int h = 0; h < height; h+=maskstep){ - int ind = h*width+w; - if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ - float z = idepth*float(depthdata[ind]); - float xn = normalsdata[3*ind+0]; - if(z > 0.2 && xn != 2){count++;} - } - } - } - + int count = model->points.size()/step; + int i = points.size()-1; if(count < 10){ is_ok[i] = false; return; @@ -203,53 +176,35 @@ void MassRegistrationPPR2::addModel(Model * model){ Eigen::Matrix & tXn = transformed_normals[i]; Eigen::VectorXd information (count); - int c = 0; - for(unsigned int w = 0; w < width; w+=maskstep){ - for(unsigned int h = 0; h < height;h+=maskstep){ - if(c == count){continue;} - int ind = h*width+w; - //if(maskvec[ind] || nomask){ - if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ - float z = idepth*float(depthdata[ind]); - float xn = normalsdata[3*ind+0]; - - if(z > 0.2 && xn != 2){ - float yn = normalsdata[3*ind+1]; - float zn = normalsdata[3*ind+2]; + for(unsigned int c = 0; c < count; c++){ - float x = (w - cx) * z * ifx; - float y = (h - cy) * z * ify; + ap[3*c+0] = model->points[c*step].point(0); + ap[3*c+1] = model->points[c*step].point(1); + ap[3*c+2] = model->points[c*step].point(2); - ap[3*c+0] =x; - ap[3*c+1] =y; - ap[3*c+2] =z; + an[3*c+0] = model->points[c*step].normal(0); + an[3*c+1] = model->points[c*step].normal(1); + an[3*c+2] = model->points[c*step].normal(2); - an[3*c+0] =xn; - an[3*c+1] =yn; - an[3*c+2] =zn; + ac[3*c+0] =model->points[c*step].feature(0); + ac[3*c+1] =model->points[c*step].feature(1); + ac[3*c+2] =model->points[c*step].feature(2); - ac[3*c+0] =rgbdata[3*ind+2]; - ac[3*c+1] =rgbdata[3*ind+1]; - ac[3*c+2] =rgbdata[3*ind+0]; + ai[c] = 1.0/model->points[c*step].point_information;//1.0/(z*z); - ai[c] = pow(fabs(z),-2);//1.0/(z*z); + X(0,c) = ap[3*c+0]; + X(1,c) = ap[3*c+1]; + X(2,c) = ap[3*c+2]; + Xn(0,c) = an[3*c+0]; + Xn(1,c) = an[3*c+1]; + Xn(2,c) = an[3*c+2]; - X(0,c) = x; - X(1,c) = y; - X(2,c) = z; - Xn(0,c) = xn; - Xn(1,c) = yn; - Xn(2,c) = zn; + information(c) = ai[c];//1.0/(z*z); + C(0,c) = ac[3*c+0]; + C(1,c) = ac[3*c+1]; + C(2,c) = ac[3*c+2]; + } - information(c) = ai[c];//1.0/(z*z); - C(0,c) = rgbdata[3*ind+0]; - C(1,c) = rgbdata[3*ind+1]; - C(2,c) = rgbdata[3*ind+2]; - c++; - } - } - } - } informations[i] = information; ArrayData3D * a3d = new ArrayData3D; @@ -259,7 +214,7 @@ void MassRegistrationPPR2::addModel(Model * model){ trees3d[i] = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); trees3d[i]->buildIndex(); - +/* int depthedge_count = 0; for(unsigned int w = 0; w < width; w++){ for(unsigned int h = 0; h < height; h++){ From f5c78cb4502cd01c3af6827f9977a9efb189526b Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Sun, 17 Jul 2016 21:30:37 +0200 Subject: [PATCH 038/255] started adding background --- .../src/segmentationserver.cpp | 2 +- .../include/modelupdater/ModelUpdater.h | 4 +- .../src/modelupdater/ModelUpdater.cpp | 94 +------------------ 3 files changed, 5 insertions(+), 95 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index 134757f4..6c75c5b6 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -125,7 +125,7 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs for(unsigned int i = 0; i < cf[i]->camera->height*cf[i]->camera->width;i++){maskdata[i] = 255;} masks.push_back(mask); } - std::vector internal_masks = mu->computeDynamicObject(0,cp,cf,masks);//Determine self occlusions + std::vector internal_masks = mu->computeDynamicObject(bg,cp,cf,masks);//Determine self occlusions std::vector dynamic_masks = mu->computeDynamicObject(bg,cp,cf,internal_masks);//Determine occlusion of background occlusions //add new frames to background //Compute minimum required frames to capture background diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 374eb740..28934286 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -90,8 +90,8 @@ namespace reglib virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); - virtual void getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask); - virtual std::vector computeDynamicObject(reglib::Model * bg, vector cp, vector cf, vector masks ); + virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask); + virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks ); virtual OcclusionScore computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1, bool debugg = false); virtual OcclusionScore computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp = Matrix4d::Identity(), int step = 1, bool debugg = false); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 30a2fda3..30b982d8 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1607,7 +1607,7 @@ printf("iter %i\n",iter); } -void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, cv::Mat mask){ +void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, cv::Mat mask){ unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); float * src_normalsdata = (float *)(frame1->normals.data); @@ -2064,96 +2064,6 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int // probs.push_back(dxy); // probs.push_back(dyx); } -/* - std::vector< std::vector > probs2; - for(unsigned int c = 0; c < 3;c++){ - std::vector Xvec; - for(unsigned int w = 10; w < width;w++){ - for(unsigned int h = 10; h < height-1;h++){ - int ind = h*width+w; - if(fabs(normalsdata[3*ind]) > 1 && fabs(normalsdata[3*(ind-10)]) > 1 && fabs(normalsdata[3*(ind-10*width)]) > 1){continue;} - Xvec.push_back(fabs(normalsdata[3*ind+c] - normalsdata[3*(ind-10*1)+c])); - Xvec.push_back(fabs(normalsdata[3*ind+c] - normalsdata[3*(ind-10*width)+c])); - } - } - - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); - for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} - - double stdval = 0; - for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} - stdval = sqrt(stdval/double(Xvec.size())); - - DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); - funcZ->zeromean = true; - funcZ->startreg = 0.025; - funcZ->debugg_print = true; - funcZ->bidir = true; - funcZ->maxd = 1.00; - funcZ->histogram_size = 1000; - funcZ->fixed_histogram_size = true; - funcZ->startmaxd = funcZ->maxd; - funcZ->starthistogram_size = funcZ->histogram_size; - funcZ->blurval = 0.5; - funcZ->stdval2 = stdval; - funcZ->maxnoise = stdval; - funcZ->reset(); - funcZ->computeModel(X); - printf("noise: %5.5f\n",funcZ->getNoise()); - - std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} - std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} - for(unsigned int w = 10; w < width;w++){ - for(unsigned int h = 10; h < height-1;h++){ - int ind = h*width+w; - if(fabs(normalsdata[3*ind]) > 1 && fabs(normalsdata[3*(ind-10*1)]) > 1 && fabs(normalsdata[3*(ind-10*width)]) > 1){continue;} - dx[ind] = funcZ->getProb(fabs(normalsdata[3*ind+c] - normalsdata[3*(ind-10*1)+c])); - dy[ind] = funcZ->getProb(fabs(normalsdata[3*ind+c] - normalsdata[3*(ind-10*width)+c])); - } - } - delete funcZ; - - probs2.push_back(dx); - probs2.push_back(dy); - } - - std::vector totprob2; totprob2.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totprob2[i] = 0.5;} - std::vector totnprob2; totnprob2.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totnprob2[i] = 0.5;} - for(unsigned int p = 0; p < probs2.size(); p++){ - for(unsigned int i = 0; i < width*height;i++){ - totprob2[i] *= probs2[p][i]; - totnprob2[i] *= 1.0- probs2[p][i]; - } - - cv::Mat joint_prob; - joint_prob.create(height,width,CV_8UC3); - unsigned char * jointdata_prob = (unsigned char *)(joint_prob.data); - for(unsigned int i = 0; i < width*height;i++){ - jointdata_prob[3*i+0] = 255.0*totprob2[i]/(totprob2[i]+totnprob2[i]); - jointdata_prob[3*i+1] = 255.0*totprob2[i]/(totprob2[i]+totnprob2[i]); - jointdata_prob[3*i+2] = 255.0*totprob2[i]/(totprob2[i]+totnprob2[i]); - } - cv::imshow( "rgb", src ); - cv::imshow( "joint_prob", joint_prob ); - cv::waitKey(0); - } - -// cv::Mat joint_prob; -// joint_prob.create(height,width,CV_8UC3); -// unsigned char * jointdata_prob = (unsigned char *)(joint_prob.data); -// for(unsigned int i = 0; i < width*height;i++){ -// jointdata_prob[3*i+0] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); -// jointdata_prob[3*i+1] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); -// jointdata_prob[3*i+2] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); -// } - -// cv::imshow( "rgb", src ); -// cv::imshow( "joint_prob", joint_prob ); -// cv::waitKey(0); - -*/ - - { std::vector Xvec; @@ -2307,7 +2217,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int return probs; } -std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, vector cp, vector cf, vector oldmasks){ +std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector oldmasks){ std::vector newmasks; for(unsigned int i = 0; i < cf.size(); i++){ From 4ab038b79ed1274646466a62a76baca0bf61d336 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 19 Jul 2016 23:19:48 +0200 Subject: [PATCH 039/255] Got the viz working --- .../include/dynamic_object_retrieval/dynamic_retrieval.h | 4 +++- .../quasimodo_retrieval/src/quasimodo_retrieval_node.cpp | 6 +++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index b3a417e5..5f358fbb 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -122,6 +122,7 @@ std::vector get_retrieved_paths(const std::vector message; @@ -130,7 +131,8 @@ std::vector get_retrieved_paths(const std::vectorsurfel_cloud, *surfel_cloud); pcl::io::savePCDFileBinary(temp_path.string(), *surfel_cloud); - retrieved_paths.push_back(temp_path); + pcl::io::savePCDFileBinary((temp_path.parent_path() / "convex_segments" / "segment.pcd").string(), *surfel_cloud); + retrieved_paths.push_back(temp_path.parent_path() / "convex_segments" / "segment.pcd"); // [0] - database, [1] - collection, [2] - mongodb object id diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index 6f4e4e39..340de280 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -239,7 +239,7 @@ class retrieval_node { //int height = 480; //int width = 640; - string mongodb_id = sweep_xml.parent_path().stem().string(); + string mongodb_id = sweep_xml.parent_path().parent_path().stem().string(); //string mongodb_uri = string("/world_state/quasimodo/") / mongodb_id; mongodb_store::MessageStoreProxy message_store_quasimodo(n, "quasimodo", "world_state"); @@ -596,7 +596,7 @@ class retrieval_node { for (int i = 0; i < retrieved_clouds.size(); ++i) { string name = results.first[i].first.stem().string(); cout << name << endl; - if (name != "surfel_map") { + if (name != "segment") { auto data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); room_transform = data.vIntermediateRoomCloudTransforms[0]; room_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0)); @@ -615,7 +615,7 @@ class retrieval_node { boost::filesystem::path segment_path = s.first; string name = segment_path.stem().string(); cout << name << endl; - if (name == "surfel_map") { // for the mongodb results + if (name == "segment") { // for the mongodb results indices.push_back(-1); } else { // for the metric map results From 8908fda8664275e8295580a8e314dbb2a3244713 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 19 Jul 2016 23:44:44 +0200 Subject: [PATCH 040/255] Fixed bug and got logging server running with placehgolders --- .../dynamic_retrieval.h | 2 +- .../src/quasimodo_logging_server.cpp | 23 ++++++++++++------- 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index 5f358fbb..216f9f03 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -116,7 +116,7 @@ std::vector get_retrieved_paths(const std::vector::loadRoomFromXML(sweep_xml.string(), vector({"RoomIntermediateCloud"}), false, false); - boost::posix_time::ptime start_time = data.roomLogStartTime; - boost::posix_time::ptime time_t_epoch(boost::gregorian::date(1970,1,1)); - boost::posix_time::time_duration diff = start_time - time_t_epoch; - Eigen::Affine3d AT; - cout << "Intermediate room cloud transforms length: " << data.vIntermediateRoomCloudTransforms.size() << endl; - tf::transformTFToEigen(data.vIntermediateRoomCloudTransforms[0], AT); - pcl_ros::transformPointCloud(AT.matrix().cast(), res.result.retrieved_clouds[i], soma_req.objects[i].cloud); + boost::posix_time::time_duration diff; + if (sweep_xml.parent_path().parent_path().stem() == "temp") { + soma_req.objects[i].cloud = res.result.retrieved_clouds[i]; + diff = boost::posix_time::time_duration(10, 10, 10); // just placeholder for now, should get real timestamp and transform from mongodb object + } + else { + auto data = SimpleXMLParser::loadRoomFromXML(sweep_xml.string(), vector({"RoomIntermediateCloud"}), false, false); + boost::posix_time::ptime start_time = data.roomLogStartTime; + boost::posix_time::ptime time_t_epoch(boost::gregorian::date(1970,1,1)); + diff = start_time - time_t_epoch; + Eigen::Affine3d AT; + cout << "Intermediate room cloud transforms length: " << data.vIntermediateRoomCloudTransforms.size() << endl; + tf::transformTFToEigen(data.vIntermediateRoomCloudTransforms[0], AT); + pcl_ros::transformPointCloud(AT.matrix().cast(), res.result.retrieved_clouds[i], soma_req.objects[i].cloud); + } //soma_req.objects[i].cloud = res.result.retrieved_clouds[i]; cout << "Retrieved initial poses length: " << res.result.retrieved_initial_poses.size() << endl; soma_req.objects[i].pose = res.result.retrieved_initial_poses[i].poses[0]; From ed1368b1f0d63f18f6e4f24fb76f1f5ffdb2703a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 27 Jul 2016 09:15:50 +0200 Subject: [PATCH 041/255] added debugg to seg --- quasimodo/quasimodo_brain/src/segmentationserver.cpp | 5 +++-- .../include/modelupdater/ModelUpdater.h | 6 +++--- .../quasimodo_models/src/modelupdater/ModelUpdater.cpp | 10 +++++----- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index 36680677..1d05d39b 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -168,7 +168,7 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs reglib::MassRegistrationPPR2 * massreg2 = new reglib::MassRegistrationPPR2(0.05); massreg2->timeout = 1200; massreg2->viewer = viewer; - massreg2->visualizationLvl = 1; + massreg2->visualizationLvl = 0; massreg2->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); massreg2->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); @@ -208,6 +208,7 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs vector bgcf; vector bgmask; + printf("time to go!\n"); //std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,cp,cf,masks,cp,cf,masks);//Determine self occlusions for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ @@ -215,7 +216,7 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs bgcf.push_back(bg->frames[k]); bgmask.push_back(bg->modelmasks[k]->getMask()); } - std::vector external_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,bgcp,bgcf,bgmask,cp,cf,masks);//Determine self occlusions + std::vector external_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,bgcp,bgcf,bgmask,cp,cf,masks,true);//Determine self occlusions /* for(unsigned int i = 0; i < cf.size(); i++){ cv::Mat mask; diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 98ccf35d..d358d55a 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -91,9 +91,9 @@ namespace reglib virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); - virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask); - virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks ); - virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks); + virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask, double debugg = false); + virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks); + virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg = false); virtual OcclusionScore computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1, bool debugg = false); virtual OcclusionScore computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp = Matrix4d::Identity(), int step = 1, bool debugg = false); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 7c4d7250..d3c640c4 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1607,7 +1607,7 @@ printf("iter %i\n",iter); } -void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, cv::Mat mask){ +void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, cv::Mat mask, double debugg){ unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); float * src_normalsdata = (float *)(frame1->normals.data); @@ -1644,7 +1644,7 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - bool debugg = true; + //bool debugg = true; pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); if(debugg){ @@ -2408,7 +2408,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int */ -vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks){ +vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg){ std::vector newmasks; for(unsigned int i = 0; i < frames.size(); i++){ @@ -2431,7 +2431,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector ModelUpdater::computeDynamicObject(vector bgcp, vector > probs = getImageProbs(cf[i],5); From f8d125b0f617784e7bc9e372180586e209a6f351 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 27 Jul 2016 17:14:45 +0200 Subject: [PATCH 042/255] small changes --- .../src/segmentationserver.cpp | 102 +++++++++++++++--- .../quasimodo_brain/src/test_segment.cpp | 39 ++++--- 2 files changed, 110 insertions(+), 31 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index 1d05d39b..cf731f46 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -27,6 +27,7 @@ using namespace std; bool visualization = false; boost::shared_ptr viewer; +//Assume internally correctly registered... //Strategy: //Register frames to background+other frames in the same model DONE @@ -44,28 +45,76 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs models.push_back(quasimodo_brain::getModelFromMSG(req.models[i])); } + reglib::Model * bg = quasimodo_brain::getModelFromMSG(req.backgroundmodel); + + reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); + massregmod->timeout = 1200; + massregmod->viewer = viewer; + massregmod->visualizationLvl = 0; + massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massregmod->nomask = true; + massregmod->stopval = 0.0005; + +/* + bg->relativeposes = bgmfr.poses; + bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + for(int j = 0; j < models.size(); j++){ + models[j]->relativeposes = bgmfr.poses; + models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + } +*/ reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); mu->occlusion_penalty = 15; mu->massreg_timeout = 60*4; mu->viewer = viewer; - reglib::Model * bg = quasimodo_brain::getModelFromMSG(req.backgroundmodel); - reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.0); - bgmassreg->timeout = 1200; - bgmassreg->viewer = viewer; - bgmassreg->visualizationLvl = 0; - bgmassreg->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); - bgmassreg->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - bgmassreg->nomask = true; - bgmassreg->stopval = 0.0005; - bgmassreg->setData(bg->frames,bg->modelmasks); - reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(bg->relativeposes); - bg->relativeposes = bgmfr.poses; + if(models.size() > 0 && bg->frames.size() > 0){ + vector cpmod; + bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + cpmod.push_back(Eigen::Matrix4d::Identity());//,models.front()->relativeposes.front().inverse() * bg->relativeposes.front()); + massregmod->addModel(bg); - bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + for(int j = 0; j < models.size(); j++){ + models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + cpmod.push_back(bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); + massregmod->addModel(models[j]); + } + + reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); + + for(int j = 0; j < models.size(); j++){ + Eigen::Matrix4d change = mfrmod.poses[j+1] * cpmod[j+1].inverse(); + std::cout << change << std::endl << std::endl << std::endl; +// for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ +// cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); +// } + } + }else if(models.size() > 1){ + vector cpmod; + + for(int j = 0; j < models.size(); j++){ + models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); + massregmod->addModel(models[j]); + } + + reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); + for(int j = 0; j < models.size(); j++){ + Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); + std::cout << change << std::endl << std::endl << std::endl; +// for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ +// cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); +// } + } + } + + + +exit(0); // printf("frames: %i\n",bg->frames.size()); // for(int i = 0; i < bg->frames.size(); i++){ // cv::imshow("modelmask", bg->modelmasks[i]->getMask()); @@ -168,7 +217,7 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs reglib::MassRegistrationPPR2 * massreg2 = new reglib::MassRegistrationPPR2(0.05); massreg2->timeout = 1200; massreg2->viewer = viewer; - massreg2->visualizationLvl = 0; + massreg2->visualizationLvl = 1; massreg2->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); massreg2->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); @@ -185,7 +234,28 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs reglib::MassFusionResults mfr2 = massreg2->getTransforms(cp); cp = mfr2.poses; +/* + reglib::MassRegistrationPPR2 * massreg3 = new reglib::MassRegistrationPPR2(0.00); + massreg3->timeout = 1200; + massreg3->viewer = viewer; + massreg3->visualizationLvl = 1; + + massreg3->maskstep = 5;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massreg3->nomaskstep = 5;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg3->nomask = true; + massreg3->stopval = 0.0005; + + massreg3->setData(cf,mm); + if(bg->frames.size() > 0){ + massreg3->addModel(bg); + cp.push_back(Eigen::Matrix4d::Identity()); + printf("---->added background model\n"); + } + + reglib::MassFusionResults mfr3 = massreg2->getTransforms(cp); + cp = mfr3.poses; +*/ Eigen::Matrix4d relative_to_bg = Eigen::Matrix4d::Identity(); if(bg->frames.size() > 0){ relative_to_bg = cp.front().inverse()*cp.back(); @@ -209,10 +279,10 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs vector bgmask; printf("time to go!\n"); - //std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,cp,cf,masks,cp,cf,masks);//Determine self occlusions + //std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,cp,cf,masks,cp,cf,masks,true);//Determine self occlusions for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ - bgcp.push_back(bg->relativeposes[k]); + bgcp.push_back(relative_to_bg*bg->relativeposes[k]); bgcf.push_back(bg->frames[k]); bgmask.push_back(bg->modelmasks[k]->getMask()); } diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index d99a4933..a3c2a657 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -702,25 +702,34 @@ int main(int argc, char** argv){ } } + //Not needed if metaroom well calibrated + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.0); + bgmassreg->timeout = 1200; + bgmassreg->viewer = viewer; + bgmassreg->visualizationLvl = 0; + bgmassreg->maskstep = 10; + bgmassreg->nomaskstep = 10; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->setData(models.front()->frames,models.front()->modelmasks); + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); + + for(int j = 0; j < models.size(); j++){ + models[j]->relativeposes = bgmfr.poses; + models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + } + + for(unsigned int i = 1; i < models.size(); i++){ quasimodo_msgs::segment_model sm; sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); if(i > 0){ - -// reglib::MassRegistrationPPR2 * massreg2 = new reglib::MassRegistrationPPR2(0.0); -// massreg2->timeout = 1200; -// massreg2->viewer = viewer; -// massreg2->visualizationLvl = 0; - -// massreg2->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); -// massreg2->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); -// massreg2->nomask = true; -// massreg2->stopval = 0.0005; - -// massreg2->setData(models[i-1]->frames,models[i-1]->modelmasks); -// reglib::MassFusionResults mfr2 = massreg2->getTransforms(models[i-1]->relativeposes); -// models[i-1]->relativeposes = mfr2.poses; - sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]); } if (segmentation_client.call(sm)){//Build model from frame From 6fc751a0ba91862c6a7e12151a2cd0018e391d33 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 29 Jul 2016 15:13:48 +0200 Subject: [PATCH 043/255] working segmentation, edges added to registration --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 10 +- .../src/segmentationserver.cpp | 136 +++- .../quasimodo_brain/src/test_segment.cpp | 12 +- quasimodo/quasimodo_models/CMakeLists.txt | 3 +- .../include/modelupdater/ModelUpdater.h | 2 + .../registration/MassRegistrationPPR2.h | 51 +- .../quasimodo_models/src/core/Camera.cpp | 4 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 13 +- .../src/modelupdater/ModelUpdater.cpp | 372 ++++++++- .../src/registration/MassRegistrationPPR2.cpp | 743 +++++++++++++----- 10 files changed, 1027 insertions(+), 319 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 4be36ecb..9201491a 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -79,9 +79,15 @@ void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Af msg.frames[startsize+i].capture_time = ros::Time(); msg.frames[startsize+i].pose = pose2; msg.frames[startsize+i].frame_id = model->frames[i]->id; - msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); + msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); - msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg());//getMask() + msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg());//getMask() + + msg.frames[startsize+i].camera.K[0] = model->frames[i]->camera->fx; + msg.frames[startsize+i].camera.K[4] = model->frames[i]->camera->fy; + msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx; + msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy; + } for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp); diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index cf731f46..cdca18c9 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -45,16 +45,32 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs models.push_back(quasimodo_brain::getModelFromMSG(req.models[i])); } +// reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.0); +// bgmassreg->timeout = 1200; +// bgmassreg->viewer = viewer; +// bgmassreg->visualizationLvl = 1; +// bgmassreg->maskstep = 10; +// bgmassreg->nomaskstep = 10; +// bgmassreg->nomask = true; +// bgmassreg->stopval = 0.0005; +// bgmassreg->setData(models.front()->frames,models.front()->modelmasks); +// reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); + +// for(int i = 0; i < models.front()->relativeposes.size(); i++){ +// models.front()->frames[i]->camera->print(); +// std::cout << models.front()->relativeposes[i] << std::endl << std::endl; +// } + reglib::Model * bg = quasimodo_brain::getModelFromMSG(req.backgroundmodel); - reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); + reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.25); massregmod->timeout = 1200; massregmod->viewer = viewer; massregmod->visualizationLvl = 0; massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massregmod->nomask = true; - massregmod->stopval = 0.0005; + massregmod->stopval = 0.0001; /* bg->relativeposes = bgmfr.poses; @@ -88,10 +104,9 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs for(int j = 0; j < models.size(); j++){ Eigen::Matrix4d change = mfrmod.poses[j+1] * cpmod[j+1].inverse(); - std::cout << change << std::endl << std::endl << std::endl; -// for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ -// cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); -// } + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + } } }else if(models.size() > 1){ vector cpmod; @@ -105,15 +120,94 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); for(int j = 0; j < models.size(); j++){ Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); - std::cout << change << std::endl << std::endl << std::endl; -// for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ -// cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); -// } + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + } + } + } + + vector bgcp; + vector bgcf; + vector bgmask; + + for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ + bgcp.push_back(bg->relativeposes[k]); + bgcf.push_back(bg->frames[k]); + bgmask.push_back(bg->modelmasks[k]->getMask()); + } + + + std::vector< std::vector< cv::Mat > > internal; + std::vector< std::vector< cv::Mat > > external; + std::vector< std::vector< cv::Mat > > dynamic; + + for(int j = 0; j < models.size(); j++){ + reglib::Model * model = models[j]; + + vector masks; + for(unsigned int i = 0; i < model->frames.size(); i++){ + reglib::RGBDFrame * frame = model->frames[i]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + masks.push_back(mask); } + + std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions + std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,true);//Determine external occlusions + std::vector dynamic_masks; + for(unsigned int i = 0; i < model->frames.size(); i++){ + reglib::RGBDFrame * frame = model->frames[i]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[i].data); + for(unsigned int k = 0; k < cam->height * cam->width;k++){ + if(externalmaskdata[k] == 0 && internalmaskdata[k] != 0 ){ + maskdata[k] = 255; + }else{ + maskdata[k] = 0; + } + } + + dynamic_masks.push_back(mask); + + cv::imshow( "rgb", frame->rgb ); + cv::imshow( "internal_masks", internal_masks[i] ); + cv::imshow( "externalmask", external_masks[i] ); + cv::imshow( "dynamic_mask", dynamic_masks[i] ); + cv::waitKey(0); + } + + internal.push_back(internal_masks); + external.push_back(external_masks); + dynamic.push_back(dynamic_masks); } +// vector updated_bgcp; +// vector updated_bgcf; +// vector updated_bgmask; + +/* + + + printf("time to go!\n"); + //std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,cp,cf,masks,cp,cf,masks,true);//Determine self occlusions + + + std::vector external_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,bgcp,bgcf,bgmask,cp,cf,masks,true);//Determine self occlusions +*/ + +/* + exit(0); // printf("frames: %i\n",bg->frames.size()); // for(int i = 0; i < bg->frames.size(); i++){ @@ -234,28 +328,7 @@ exit(0); reglib::MassFusionResults mfr2 = massreg2->getTransforms(cp); cp = mfr2.poses; -/* - reglib::MassRegistrationPPR2 * massreg3 = new reglib::MassRegistrationPPR2(0.00); - massreg3->timeout = 1200; - massreg3->viewer = viewer; - massreg3->visualizationLvl = 1; - - massreg3->maskstep = 5;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massreg3->nomaskstep = 5;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massreg3->nomask = true; - massreg3->stopval = 0.0005; - - massreg3->setData(cf,mm); - - if(bg->frames.size() > 0){ - massreg3->addModel(bg); - cp.push_back(Eigen::Matrix4d::Identity()); - printf("---->added background model\n"); - } - reglib::MassFusionResults mfr3 = massreg2->getTransforms(cp); - cp = mfr3.poses; -*/ Eigen::Matrix4d relative_to_bg = Eigen::Matrix4d::Identity(); if(bg->frames.size() > 0){ relative_to_bg = cp.front().inverse()*cp.back(); @@ -287,6 +360,7 @@ exit(0); bgmask.push_back(bg->modelmasks[k]->getMask()); } std::vector external_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,bgcp,bgcf,bgmask,cp,cf,masks,true);//Determine self occlusions +*/ /* for(unsigned int i = 0; i < cf.size(); i++){ cv::Mat mask; diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index a3c2a657..5b4a338d 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -712,7 +712,9 @@ int main(int argc, char** argv){ reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.0); bgmassreg->timeout = 1200; bgmassreg->viewer = viewer; - bgmassreg->visualizationLvl = 0; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = true; + bgmassreg->visualizationLvl = 1; bgmassreg->maskstep = 10; bgmassreg->nomaskstep = 10; bgmassreg->nomask = true; @@ -720,11 +722,18 @@ int main(int argc, char** argv){ bgmassreg->setData(models.front()->frames,models.front()->modelmasks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); + + for(int j = 0; j < models.size(); j++){ models[j]->relativeposes = bgmfr.poses; models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); } +// for(int i = 0; i < models.front()->relativeposes.size(); i++){ +// models.front()->frames[i]->camera->print(); +// std::cout << models.front()->relativeposes[i] << std::endl << std::endl; +// } + for(unsigned int i = 1; i < models.size(); i++){ quasimodo_msgs::segment_model sm; @@ -749,7 +758,6 @@ int main(int argc, char** argv){ // ROS_INFO("model_id%i", model_id ); // } // }else{ROS_ERROR("Failed to call service index_frame");} -exit(0); /* ros::NodeHandle pn("~"); diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 2d878583..9891b230 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -52,6 +52,7 @@ include_directories(/usr/local/include/) FIND_PACKAGE(Ceres REQUIRED) INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) +add_subdirectory (densecrf) add_library(quasimodo_weightlib src/weightfunctions/DistanceWeightFunction2.cpp src/weightfunctions/DistanceWeightFunction2PPR.cpp src/weightfunctions/DistanceWeightFunction2PPR2.cpp) target_link_libraries(quasimodo_weightlib ceres glog ${catkin_LIBRARIES}) @@ -75,7 +76,7 @@ add_library(quasimodo_Mesher src/mesher/Mesh.cpp) target_link_libraries(quasimodo_Mesher quasimodo_model ${catkin_LIBRARIES}) add_library(quasimodo_ModelUpdater src/modelupdater/ModelUpdater.cpp src/modelupdater/ModelUpdaterBasic.cpp src/modelupdater/ModelUpdaterBasicFuse.cpp) -target_link_libraries(quasimodo_ModelUpdater quasimodo_Mesher quasimodo_model quasimodo_reglib quasimodo_massreglib ${catkin_LIBRARIES}) +target_link_libraries(quasimodo_ModelUpdater densecrf quasimodo_Mesher quasimodo_model quasimodo_reglib quasimodo_massreglib ${catkin_LIBRARIES}) diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index d358d55a..ccd885d8 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -22,6 +22,8 @@ #include +//#include "../../densecrf/include/densecrf.h" + namespace reglib { using namespace std; diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h index 1e4035af..4630471c 100644 --- a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h +++ b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h @@ -48,27 +48,13 @@ namespace reglib double total_time_start; const unsigned int maxcount = 1000000; - double * Qp_arr; - double * Qn_arr; - double * Xp_arr; - double * Xn_arr; - double * rangeW_arr; - - double * kp_Qp_arr; - double * kp_Qn_arr; - double * kp_Xp_arr; - double * kp_Xn_arr; - double * kp_rangeW_arr; - - double * depthedge_Qp_arr; - double * depthedge_Xp_arr; - double * depthedge_rangeW_arr; Model * model; std::vector nr_datas; std::vector< bool > is_ok; + std::vector< Eigen::Matrix > points; std::vector< Eigen::Matrix > colors; std::vector< Eigen::Matrix > normals; @@ -76,30 +62,42 @@ namespace reglib std::vector< Eigen::Matrix > transformed_normals; std::vector< Eigen::VectorXd > informations; + + std::vector< int > kp_nr_arraypoints; std::vector< double * > kp_arraypoints; std::vector< double * > kp_arraynormals; std::vector< double * > kp_arrayinformations; std::vector< uint64_t * > kp_arraydescriptors; std::vector< std::vector< std::vector< TestMatch > > > kp_matches; - + double * kp_Qp_arr; + double * kp_Qn_arr; + double * kp_Xp_arr; + double * kp_Xn_arr; + double * kp_rangeW_arr; + DistanceWeightFunction2PPR2 * kpfunc; //std::vector< std::vector< std::vector > > matchids; - bool use_surface; std::vector< int > frameid; + bool use_surface; std::vector< int > nr_arraypoints; std::vector< double * > arraypoints; std::vector< double * > arraynormals; std::vector< double * > arraycolors; std::vector< double * > arrayinformations; - std::vector< Tree3d * > trees3d; std::vector< ArrayData3D * > a3dv; - std::vector nr_matches; std::vector< std::vector< std::vector > > matchids; std::vector< std::vector< std::vector > > matchdists; + double * Qp_arr; + double * Qn_arr; + double * Xp_arr; + double * Xn_arr; + double * rangeW_arr; + DistanceWeightFunction2PPR2 * func; + std::vector< std::vector< double > > matchscores; bool use_depthedge; @@ -108,11 +106,13 @@ namespace reglib std::vector< double * > depthedge_arrayinformations; std::vector< Tree3d * > depthedge_trees3d; std::vector< ArrayData3D * > depthedge_a3dv; - std::vector depthedge_nr_matches; std::vector< std::vector< std::vector > > depthedge_matchids; std::vector< std::vector< std::vector > > depthedge_matchdists; - + double * depthedge_Qp_arr; + double * depthedge_Xp_arr; + double * depthedge_rangeW_arr; + DistanceWeightFunction2PPR2 * depthedge_func; std::vector sweepids; std::vector background_nr_datas; @@ -131,9 +131,8 @@ namespace reglib // std::vector feature_end;//Dimension of data a specific feature ends, if the feature is RGB this should be 5 // std::vector< DistanceWeightFunction2 * > feature_func; - DistanceWeightFunction2PPR2 * func; - DistanceWeightFunction2PPR2 * kpfunc; - DistanceWeightFunction2PPR2 * depthdege_func; + + MassRegistrationPPR2(double startreg = 0.05, bool visualize = false); ~MassRegistrationPPR2(); @@ -151,8 +150,12 @@ namespace reglib void rematchKeyPoints(std::vector poses, std::vector prev_poses, bool first); void rematch(std::vector poses, std::vector prev_poses, bool first); Eigen::MatrixXd getAllResiduals(std::vector poses); + Eigen::MatrixXd depthedge_getAllResiduals(std::vector poses); Eigen::MatrixXd getAllKpResiduals(std::vector poses); + + void showEdges(std::vector poses); + //Eigen::MatrixXd getAllKPResiduals(std::vector poses); std::vector optimize(std::vector poses); diff --git a/quasimodo/quasimodo_models/src/core/Camera.cpp b/quasimodo/quasimodo_models/src/core/Camera.cpp index 54dd231d..3c271050 100644 --- a/quasimodo/quasimodo_models/src/core/Camera.cpp +++ b/quasimodo/quasimodo_models/src/core/Camera.cpp @@ -58,7 +58,9 @@ void Camera::save(std::string path){ delete[] buffer; } -void Camera::print(){} +void Camera::print(){ + printf("Camera fx: %5.5f fy: %5.5f cx: %5.5f cy: %5.5f\n",fx,fy,cx,cy); +} Camera * Camera::load(std::string path){ Camera * cam = new Camera(); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 22483f88..66e4ddac 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -368,24 +368,25 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt // out.data[3*ind+2] =255; } - for(unsigned int i = 0; i < label_indices[3].indices.size(); i++){ - int ind = label_indices[3].indices[i]; - depthedgesdata[ind] = 255; +// for(unsigned int i = 0; i < label_indices[3].indices.size(); i++){ +// int ind = label_indices[3].indices[i]; +// depthedgesdata[ind] = 255; // out.data[3*ind+0] =0; // out.data[3*ind+1] =255; // out.data[3*ind+2] =255; - } +// } for(unsigned int i = 0; i < label_indices[4].indices.size(); i++){ int ind = label_indices[4].indices[i]; - //depthedgesdata[ind] = 255; + depthedgesdata[ind] = 255; // out.data[3*ind+0] =0; // out.data[3*ind+1] =255; // out.data[3*ind+2] =0; } -// cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", out ); +// cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "edges", out ); // cv::waitKey(0); //show(true); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index d3c640c4..78790c5d 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2409,17 +2409,27 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int */ vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg){ - std::vector newmasks; + std::vector newmasks; + + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + std::vector unaryprobs; + double maxprob = 0.7; + + pcl::PointCloud::Ptr cloud1 (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); for(unsigned int i = 0; i < frames.size(); i++){ - unsigned short * depthdata = (unsigned short *)(frames[i]->depth.data); + RGBDFrame * frame = frames[i]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - Camera * camera = frames[i]->camera; + Camera * camera = frame->camera; const unsigned int width = camera->width; const unsigned int height = camera->height; - unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; + unsigned int nr_pixels = frame->camera->width * frame->camera->height; double * overlaps = new double[nr_pixels]; double * occlusions = new double[nr_pixels]; for(unsigned int j = 0; j < nr_pixels; j++){ @@ -2429,9 +2439,9 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector ModelUpdater::computeDynamicObject(vector bgcp, vector > probs = getImageProbs(cf[i],5); + std::vector< std::vector > probs = getImageProbs(frames[i],5); printf("starting partition\n"); double start_part = getTime(); int nr_data = width*height; gc::Graph *g = new gc::Graph( nr_data, width*(height-1) + (width-1)*height); - double maxprob = 0.7; + for(unsigned int j = 0; j < nr_data;j++){ g -> add_node(); if(depthdata[j] == 0){ @@ -2458,7 +2468,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector= 1){ p_fg = maxprob;} else if(overlaps[j] >= 1){ p_fg = 0.4;} @@ -2472,37 +2482,40 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector 0){ - double ay = 0.5; - double by = 0.5; - for(int p = 1; p < probs.size(); p+=2){ - double pr = probs[p][ind]; - ay *= pr; - by *= 1.0-pr; - } - double py = ay/(ay+by); - if(w > 0){ + double ax = 0.5; + double bx = 0.5; + for(int p = 0; p < probs.size(); p+=2){ + double pr = probs[p][ind]; + ax *= pr; + bx *= 1.0-pr; + } + double px = ax/(ax+bx); + int other = ind-1; - double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); + double p_same = std::max(probs[probs.size()-2][ind+1],std::min(px,maxprob_same)); double not_p_same = 1-p_same; double weight = -log(not_p_same); - g -> add_edge( ind, other, weight, weight ); + g -> add_edge( ind, other, weight, weight ); } if(h > 0){ + + double ay = 0.5; + double by = 0.5; + for(int p = 1; p < probs.size(); p+=2){ + double pr = probs[p][ind]; + ay *= pr; + by *= 1.0-pr; + } + double py = ay/(ay+by); + int other = ind-width; double p_same = std::max(probs[probs.size()-1][ind],std::min(py,maxprob_same)); double not_p_same = 1-p_same; @@ -2519,22 +2532,305 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vectoridepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + pcl::PointXYZRGBNormal point; + point.x = m00*x + m01*y + m02*z + m03; + point.y = m10*x + m11*y + m12*z + m13; + point.z = m20*x + m21*y + m22*z + m23; + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; + + cloud->points.push_back(point); + + double p_fg = 0.49999; + + if(occlusions[ind] >= 1){ p_fg = maxprob;} + else if(overlaps[ind] >= 1){ p_fg = 0.4;} + + p_fg = std::max(1-maxprob,std::min(maxprob,p_fg)); + + + unaryprobs.push_back(p_fg); + } + } + } + + bool debugg1 = debugg; + if(debugg1){ + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + if (g->what_segment(ind) == gc::Graph::SOURCE){ + point.r = 0; + point.g = 255; + point.b = 0; + }else{ + point.r = 255; + point.g = 0; + point.b = 0; + } + //cloud->points[ind] = point; + cloud1->points.push_back(point); + } + } + } + } + + bool debugg2 = debugg; + if(debugg2){ + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + + point.r = 0; + point.g = 255; + point.b = 255; + + if(overlaps[ind] >= 1){ + point.r = 0; + point.g = 255; + point.b = 0; + } + if(occlusions[ind] >= 1){ + point.r = 255; + point.g = 0; + point.b = 0; + } + //cloud->points[ind] = point; + cloud2->points.push_back(point); + } + } + } + } + - internalmask.create(height,width,CV_8UC1); - unsigned char * internaldata = (unsigned char *)(internalmask.data); - for(unsigned int i = 0; i < width*height;i++){internaldata[i] = 255.0*(g->what_segment(i) == gc::Graph::SOURCE);} -// cv::imshow( "rgb", cf[i]->rgb ); -// cv::imshow( "externalmask", internalmask ); -// cv::waitKey(0); + cv::Mat internalmask; + internalmask.create(height,width,CV_8UC1); + unsigned char * internaldata = (unsigned char *)(internalmask.data); + for(unsigned int i = 0; i < width*height;i++){ + internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); + } newmasks.push_back(internalmask); //Time to compute external masks... delete[] overlaps; delete[] occlusions; } +/* + if(debugg){ + for(double weight = 2000; weight < 10000; weight *= 2){ + printf("weight: %f\n",weight); + DenseCRF crf (cloud->points.size(),2); + + double poseScale = 0.1; + //double jointColorScale = 20.0; + //double jointPoseScale = 0.05; + MatrixXf featureJoint( 6, cloud->points.size() ); + MatrixXf featurePose( 3, cloud->points.size() ); + MatrixXf unary( 2, cloud->points.size() ); + for(unsigned int k=0; k < unaryprobs.size(); k++ ){ + double p_fg = unaryprobs[k]; + double p_bg = 1-p_fg; + + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + + unary(0,k) = weightFG/weight; + unary(1,k) = weightBG/weight; + + pcl::PointXYZRGBNormal point = cloud->points[k]; + featureJoint(0,k) = point.x/0.05; + featureJoint(1,k) = point.y/0.05; + featureJoint(2,k) = point.z/0.05; + featureJoint(3,k) = point.r/60.0; + featureJoint(4,k) = point.g/60.0; + featureJoint(5,k) = point.b/60.0; + + featurePose(0,k) = point.x/0.001; + featurePose(1,k) = point.y/0.001; + featurePose(2,k) = point.z/0.001; + } + + crf.setUnaryEnergy( unary ); + crf.addPairwiseEnergy(featureJoint,new PottsCompatibility( 10 ),DIAG_KERNEL,NORMALIZE_SYMMETRIC); + crf.addPairwiseEnergy(featurePose,new PottsCompatibility( 10 ),DIAG_KERNEL,NORMALIZE_SYMMETRIC); + VectorXs map = crf.map(25); + + + + pcl::PointCloud::Ptr cloud3 (new pcl::PointCloud); + for(unsigned int k = 0; k < cloud->points.size(); k++ ){ + pcl::PointXYZRGBNormal point = cloud->points[k]; + if( map(k) == 0 ){ + point.r = 255; + point.g = 0; + point.b = 0; + }else{ + point.r = 0; + point.g = 255; + point.b = 0; + } + cloud3->points.push_back(point); + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud3, pcl::visualization::PointCloudColorHandlerRGBField(cloud3), "scloud"); + viewer->spin(); + } + } +*/ + /* + + // add a color dependent term (feature = xyrgb) + // x_stddev = 60 + // y_stddev = 60 + // r_stddev = g_stddev = b_stddev = 20 + // weight = 10 + + + crf.addPairwiseGaussian( 3, 3, new PottsCompatibility( 3 ) ); + void DenseCRF2D::addPairwiseGaussian ( float sx, float sy, LabelCompatibility * function, KernelType kernel_type, NormalizationType normalization_type ) { + MatrixXf feature( 2, N_ ); + for( int j=0; jremoveAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud1, pcl::visualization::PointCloudColorHandlerRGBField(cloud1), "scloud"); + viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "scloud"); + viewer->spin(); + + +// pcl::PointCloud::Ptr cloud3 (new pcl::PointCloud); +// for(unsigned int k=0; k < unaryprobs.size(); k++ ){ +// pcl::PointXYZRGBNormal point = cloud->points[k]; +// if(map(k) == 0){ +// point.r = 255; +// point.g = 0; +// point.b = 0; +// }else{ +// point.r = 0; +// point.g = 255; +// point.b = 0; +// } +// cloud3->points.push_back(point); +// } + +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud3, pcl::visualization::PointCloudColorHandlerRGBField(cloud3), "scloud"); +// viewer->spin(); + } return newmasks; } diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 2a6f9f59..9c0b0707 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -37,7 +37,7 @@ MassRegistrationPPR2::MassRegistrationPPR2(double startreg, bool visualize){ depthedge_dwf->update_size = true; depthedge_dwf->startreg = startreg; depthedge_dwf->debugg_print = false; - depthdege_func = depthedge_dwf; + depthedge_func = depthedge_dwf; fast_opt = true; @@ -90,6 +90,7 @@ MassRegistrationPPR2::~MassRegistrationPPR2(){ delete func; delete kpfunc; + delete depthedge_func; delete[] Qp_arr; delete[] Qn_arr; @@ -130,6 +131,10 @@ void MassRegistrationPPR2::addModel(Model * model){ trees3d.push_back(0); a3dv.push_back(0); + depthedge_nr_matches.push_back( 0); + depthedge_matchids.push_back( std::vector< std::vector >() ); + depthedge_matchdists.push_back( std::vector< std::vector >() ); + depthedge_nr_arraypoints.push_back(0); depthedge_arraypoints.push_back(0); depthedge_arrayinformations.push_back(0); @@ -565,76 +570,35 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect for(unsigned int i = 0; i < nr_frames; i++){ if(!is_ok[i]){continue;} nr_matches[i] = 0; + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + if(!first){ + Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); - if(use_depthedge){ + Eigen::Affine3d diff = prev_rp.inverse()*rp; - } - if(use_surface){ - for(unsigned int j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - if(i == j){continue;} - Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); - if(!first){ - Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); - - Eigen::Affine3d diff = prev_rp.inverse()*rp; - - double change_trans = 0; - double change_rot = 0; - double dt = 0; - for(unsigned int k = 0; k < 3; k++){ - dt += diff(k,3)*diff(k,3); - for(unsigned int l = 0; l < 3; l++){ - if(k == l){ change_rot += fabs(1-diff(k,l));} - else{ change_rot += fabs(diff(k,l));} - } + double change_trans = 0; + double change_rot = 0; + double dt = 0; + for(unsigned int k = 0; k < 3; k++){ + dt += diff(k,3)*diff(k,3); + for(unsigned int l = 0; l < 3; l++){ + if(k == l){ change_rot += fabs(1-diff(k,l));} + else{ change_rot += fabs(diff(k,l));} } - change_trans += sqrt(dt); - - //if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;} } + change_trans += sqrt(dt); - //prev_poses[j] = poses[j]; - matchframes(rp, nr_arraypoints[i], arraypoints[i], matchids[i][j], matchdists[i][j],trees3d[j], new_good_rematches,new_total_rematches); -/* - const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); - const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); - const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); - - const unsigned int nr_ap = nr_arraypoints[i]; - double * ap = arraypoints[i]; - - std::vector & matchid = matchids[i][j]; - std::vector & matchdist = matchdists[i][j]; - matchid.resize(nr_ap); - matchdist.resize(nr_ap); - Tree3d * t3d = trees3d[j]; + //if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;} + } -#pragma omp parallel for num_threads(8) - for(unsigned int k = 0; k < nr_ap; ++k) { - int prev = matchid[k]; - double qp [3]; - const double & src_x = ap[3*k+0]; - const double & src_y = ap[3*k+1]; - const double & src_z = ap[3*k+2]; - - qp[0] = m00*src_x + m01*src_y + m02*src_z + m03; - qp[1] = m10*src_x + m11*src_y + m12*src_z + m13; - qp[2] = m20*src_x + m21*src_y + m22*src_z + m23; - - size_t ret_index; double out_dist_sqr; - nanoflann::KNNResultSet resultSet(1); - resultSet.init(&ret_index, &out_dist_sqr ); - t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); - - int current = ret_index; - new_good_rematches += prev != current; - new_total_rematches++; - matchid[k] = current; - matchdist[k] = out_dist_sqr; - } - nr_matches[i] += matchid.size(); - */ + if(use_depthedge){ + matchframes(rp, depthedge_nr_arraypoints[i], depthedge_arraypoints[i], depthedge_matchids[i][j], depthedge_matchdists[i][j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); + } + if(use_surface){ + matchframes(rp, nr_arraypoints[i], arraypoints[i], matchids[i][j], matchdists[i][j],trees3d[j], new_good_rematches,new_total_rematches); } } } @@ -833,8 +797,8 @@ Eigen::MatrixXd MassRegistrationPPR2::getAllResiduals(std::vector poses){ + unsigned int nr_frames = poses.size(); + Eigen::MatrixXd all_residuals; + + int total_matches = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + total_matches += depthedge_matchids[i][j].size(); + } + } + + + all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches); + + + int count = 0; + for(unsigned int i = 0; i < nr_frames; i++){ + if(!is_ok[i]){continue;} + + double * api = depthedge_arraypoints[i]; + double * aii = depthedge_arrayinformations[i]; + const unsigned int nr_api = depthedge_nr_arraypoints[i]; + for(unsigned int j = 0; j < nr_frames; j++){ + if(!is_ok[j]){continue;} + if(i == j){continue;} + + double * apj = depthedge_arraypoints[j]; + double * aij = depthedge_arrayinformations[j]; + const unsigned int nr_apj = depthedge_nr_arraypoints[j]; + + std::vector & matchidi = depthedge_matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + + for(unsigned int ki = 0; ki < nr_api; ++ki) { + int kj = matchidi[ki]; + if( kj < 0 ){continue;} + const double & src_x = api[3*ki+0]; + const double & src_y = api[3*ki+1]; + const double & src_z = api[3*ki+2]; + + const double & info_i = aii[ki]; + + const double & dst_x = apj[3*kj+0]; + const double & dst_y = apj[3*kj+1]; + const double & dst_z = apj[3*kj+2]; + + const double & info_j = aij[kj]; + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + const double rw = 1.0/(1.0/info_i+1.0/info_j); + all_residuals(0,count) = (tx-dst_x)*rw; + all_residuals(1,count) = (ty-dst_y)*rw; + all_residuals(2,count) = (tz-dst_z)*rw; + count++; + } + } + } + return all_residuals; +} + Eigen::MatrixXd MassRegistrationPPR2::getAllKpResiduals(std::vector poses){ unsigned int nr_frames = poses.size(); @@ -1014,11 +1049,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ frames.push_back(frame); mmasks.push_back(mmask); - nr_matches.push_back( 0); - matchids.push_back( std::vector< std::vector >() ); - matchdists.push_back( std::vector< std::vector >() ); nr_datas.push_back( 0); - points.push_back( Eigen::Matrix()); colors.push_back( Eigen::Matrix()); normals.push_back( Eigen::Matrix()); @@ -1027,6 +1058,10 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ informations.push_back( Eigen::VectorXd()); + nr_matches.push_back( 0); + matchids.push_back( std::vector< std::vector >() ); + matchdists.push_back( std::vector< std::vector >() ); + nr_arraypoints.push_back(0); arraypoints.push_back(0); arraynormals.push_back(0); @@ -1035,6 +1070,10 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ trees3d.push_back(0); a3dv.push_back(0); + depthedge_nr_matches.push_back( 0); + depthedge_matchids.push_back( std::vector< std::vector >() ); + depthedge_matchdists.push_back( std::vector< std::vector >() ); + depthedge_nr_arraypoints.push_back(0); depthedge_arraypoints.push_back(0); depthedge_arrayinformations.push_back(0); @@ -1165,8 +1204,8 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ int depthedge_count = 0; - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height; h++){ + for(unsigned int w = 0; w < width; w+=2){ + for(unsigned int h = 0; h < height; h+=2){ int ind = h*width+w; if(maskvec[ind] && edgedata[ind] == 255){ float z = idepth*float(depthdata[ind]); @@ -1175,7 +1214,8 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ } } - if(depthedge_count < 10){ + printf("depthedge_count: %i\n",depthedge_count); + if(depthedge_count > 10){ double * depthedge_ap = new double[3*depthedge_count]; double * depthedge_ai = new double[3*depthedge_count]; depthedge_nr_arraypoints[i] = depthedge_count; @@ -1183,8 +1223,8 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ depthedge_arrayinformations[i] = depthedge_ai; c = 0; - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height; h++){ + for(unsigned int w = 0; w < width; w+=1){ + for(unsigned int h = 0; h < height; h+=1){ if(c == depthedge_count){continue;} int ind = h*width+w; if(maskvec[ind] && edgedata[ind] == 255){ @@ -1208,7 +1248,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ depthedge_trees3d[i]->buildIndex(); } - //printf("total load time: %5.5f\n",getTime()-total_load_time_start); + printf("total load time: %5.5f\n",getTime()-total_load_time_start); } void MassRegistrationPPR2::addData(pcl::PointCloud::Ptr cloud){ @@ -1340,9 +1380,10 @@ void MassRegistrationPPR2::addData(pcl::PointCloud::Ptr a3dv[i] = a3d; trees3d[i] = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); trees3d[i]->buildIndex(); - if(visualizationLvl > 0){ - printf("total load time: %5.5f\n",getTime()-total_load_time_start); - } +// if(visualizationLvl > 0){ +// printf("total load time: %5.5f\n",getTime()-total_load_time_start); +// } + printf("total load time: %5.5f\n",getTime()-total_load_time_start); } std::vector MassRegistrationPPR2::optimize(std::vector poses){ @@ -1375,6 +1416,11 @@ std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector & matchidj = matchids[j][i]; - unsigned int matchesj = matchidj.size(); - std::vector & matchidi = matchids[i][j]; - unsigned int matchesi = matchidi.size(); + if(use_surface){ + std::vector & matchidj = matchids[j][i]; + unsigned int matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned int matchesi = matchidi.size(); - std::vector & matchdistj = matchdists[j][i]; - std::vector & matchdisti = matchdists[i][j]; + std::vector & matchdistj = matchdists[j][i]; + std::vector & matchdisti = matchdists[i][j]; - for(unsigned int ki = 0; true && ki < matchesi; ki++){ - int kj = matchidi[ki]; - if( kj == -1 ){continue;} - if( kj >= matchesj){continue;} - if(!onetoone || matchidj[kj] == ki){ - Xp_arr[3*count+0] = api[3*ki+0]; - Xp_arr[3*count+1] = api[3*ki+1]; - Xp_arr[3*count+2] = api[3*ki+2]; + for(unsigned int ki = 0; true && ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( kj == -1 ){continue;} + if( kj >= matchesj){continue;} + if(!onetoone || matchidj[kj] == ki){ + Xp_arr[3*count+0] = api[3*ki+0]; + Xp_arr[3*count+1] = api[3*ki+1]; + Xp_arr[3*count+2] = api[3*ki+2]; - Xn_arr[3*count+0] = ani[3*ki+0]; - Xn_arr[3*count+1] = ani[3*ki+1]; - Xn_arr[3*count+2] = ani[3*ki+2]; + Xn_arr[3*count+0] = ani[3*ki+0]; + Xn_arr[3*count+1] = ani[3*ki+1]; + Xn_arr[3*count+2] = ani[3*ki+2]; - const double & info_i = aii[ki]; + const double & info_i = aii[ki]; - const double & dst_x = apj[3*kj+0]; - const double & dst_y = apj[3*kj+1]; - const double & dst_z = apj[3*kj+2]; + const double & dst_x = apj[3*kj+0]; + const double & dst_y = apj[3*kj+1]; + const double & dst_z = apj[3*kj+2]; - const double & dst_nx = anj[3*kj+0]; - const double & dst_ny = anj[3*kj+1]; - const double & dst_nz = anj[3*kj+2]; + const double & dst_nx = anj[3*kj+0]; + const double & dst_ny = anj[3*kj+1]; + const double & dst_nz = anj[3*kj+2]; - const double & info_j = aij[kj]; + const double & info_j = aij[kj]; - Qp_arr[3*count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03; - Qp_arr[3*count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13; - Qp_arr[3*count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23; + Qp_arr[3*count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03; + Qp_arr[3*count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13; + Qp_arr[3*count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23; - Qn_arr[3*count+0] = mj00*dst_nx + mj01*dst_ny + mj02*dst_nz; - Qn_arr[3*count+1] = mj10*dst_nx + mj11*dst_ny + mj12*dst_nz; - Qn_arr[3*count+2] = mj20*dst_nx + mj21*dst_ny + mj22*dst_nz; + Qn_arr[3*count+0] = mj00*dst_nx + mj01*dst_ny + mj02*dst_nz; + Qn_arr[3*count+1] = mj10*dst_nx + mj11*dst_ny + mj12*dst_nz; + Qn_arr[3*count+2] = mj20*dst_nx + mj21*dst_ny + mj22*dst_nz; - rangeW_arr[count] = 1.0/(1.0/info_i+1.0/info_j); + rangeW_arr[count] = 1.0/(1.0/info_i+1.0/info_j); - count++; + count++; + } + } + } + + + if(use_depthedge){ + std::vector & matchidj = depthedge_matchids[j][i]; + unsigned int matchesj = matchidj.size(); + std::vector & matchidi = depthedge_matchids[i][j]; + unsigned int matchesi = matchidi.size(); + + + std::vector & matchdistj = depthedge_matchdists[j][i]; + std::vector & matchdisti = depthedge_matchdists[i][j]; + + for(unsigned int ki = 0; true && ki < matchesi; ki++){ + int kj = matchidi[ki]; + if( kj == -1 ){continue;} + if( kj >= matchesj){continue;} + if(!onetoone || matchidj[kj] == ki){ + depthedge_Xp_arr[3*depthedge_count+0] = depthedge_api[3*ki+0]; + depthedge_Xp_arr[3*depthedge_count+1] = depthedge_api[3*ki+1]; + depthedge_Xp_arr[3*depthedge_count+2] = depthedge_api[3*ki+2]; + + const double & info_i = depthedge_aii[ki]; + + const double & dst_x = depthedge_apj[3*kj+0]; + const double & dst_y = depthedge_apj[3*kj+1]; + const double & dst_z = depthedge_apj[3*kj+2]; + + const double & info_j = depthedge_aij[kj]; + + depthedge_Qp_arr[3*depthedge_count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03; + depthedge_Qp_arr[3*depthedge_count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13; + depthedge_Qp_arr[3*depthedge_count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23; + + depthedge_rangeW_arr[depthedge_count] = 1.0/(1.0/info_i+1.0/info_j); + + depthedge_count++; + } } } /* @@ -1489,7 +1580,9 @@ std::vector MassRegistrationPPR2::optimize(std::vector Vector6d; typedef Eigen::Matrix Matrix6d; @@ -1518,105 +1611,221 @@ std::vector MassRegistrationPPR2::optimize(std::vectorgetNoise(); + double depthedge_kpInfo = 1.0/(depthedge_Noise*depthedge_Noise); + double kpNoise = kpfunc->getNoise(); double kpInfo = 1.0/(kpNoise*kpNoise); - for(unsigned int co = 0; co < count; co++){ - const double & src_x = Xp_arr[3*co+0]; - const double & src_y = Xp_arr[3*co+1]; - const double & src_z = Xp_arr[3*co+2]; - const double & src_nx = Xn_arr[3*co+0]; - const double & src_ny = Xn_arr[3*co+1]; - const double & src_nz = Xn_arr[3*co+2]; - - const double & dx = Qp_arr[3*co+0]; - const double & dy = Qp_arr[3*co+1]; - const double & dz = Qp_arr[3*co+2]; - const double & dnx = Qn_arr[3*co+0]; - const double & dny = Qn_arr[3*co+1]; - const double & dnz = Qn_arr[3*co+2]; - - const double & rw = rangeW_arr[co]; + if(use_surface){ + for(unsigned int co = 0; co < count; co++){ + const double & src_x = Xp_arr[3*co+0]; + const double & src_y = Xp_arr[3*co+1]; + const double & src_z = Xp_arr[3*co+2]; + const double & src_nx = Xn_arr[3*co+0]; + const double & src_ny = Xn_arr[3*co+1]; + const double & src_nz = Xn_arr[3*co+2]; + + const double & dx = Qp_arr[3*co+0]; + const double & dy = Qp_arr[3*co+1]; + const double & dz = Qp_arr[3*co+2]; + const double & dnx = Qn_arr[3*co+0]; + const double & dny = Qn_arr[3*co+1]; + const double & dnz = Qn_arr[3*co+2]; + + const double & rw = rangeW_arr[co]; + + const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; + const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; + const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; + + const double & nx = m00*src_nx + m01*src_ny + m02*src_nz; + const double & ny = m10*src_nx + m11*src_ny + m12*src_nz; + const double & nz = m20*src_nx + m21*src_ny + m22*src_nz; + + const double & angle = nx*dnx+ny*dny+nz*dnz; + //printf("%f\n",angle); + //if(angle < 0){exit(0);continue;} + + if(angle < 0){continue;} + + double di = rw*(nx*(sx-dx) + ny*(sy-dy) + nz*(sz-dz)); + //double weight = angle*angle*angle*angle*func->getProb(di)*rw*rw; + double prob = func->getProb(di); + double weight = planeInfo*prob*rw*rw; + + if(false && visualizationLvl == 5){ + pcl::PointXYZRGBNormal p; + p.x = sx; + p.y = sy; + p.z = sz; + p.b = 0; + p.g = 255; + p.r = 0; + scloud->points.push_back(p); + + pcl::PointXYZRGBNormal p1; + p1.x = dx; + p1.y = dy; + p1.z = dz; + p1.b = 255.0*prob; + p1.g = 255.0*prob; + p1.r = 255.0*prob; + dcloud->points.push_back(p1); + } - const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; - const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; - const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; + const double & a = nz*sy - ny*sz; + const double & b = nx*sz - nz*sx; + const double & c = ny*sx - nx*sy; + + ATA.coeffRef (0) += weight * a * a; + ATA.coeffRef (1) += weight * a * b; + ATA.coeffRef (2) += weight * a * c; + ATA.coeffRef (3) += weight * a * nx; + ATA.coeffRef (4) += weight * a * ny; + ATA.coeffRef (5) += weight * a * nz; + ATA.coeffRef (7) += weight * b * b; + ATA.coeffRef (8) += weight * b * c; + ATA.coeffRef (9) += weight * b * nx; + ATA.coeffRef (10) += weight * b * ny; + ATA.coeffRef (11) += weight * b * nz; + ATA.coeffRef (14) += weight * c * c; + ATA.coeffRef (15) += weight * c * nx; + ATA.coeffRef (16) += weight * c * ny; + ATA.coeffRef (17) += weight * c * nz; + ATA.coeffRef (21) += weight * nx * nx; + ATA.coeffRef (22) += weight * nx * ny; + ATA.coeffRef (23) += weight * nx * nz; + ATA.coeffRef (28) += weight * ny * ny; + ATA.coeffRef (29) += weight * ny * nz; + ATA.coeffRef (35) += weight * nz * nz; + + const double & d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz); + + ATb.coeffRef (0) += a * d; + ATb.coeffRef (1) += b * d; + ATb.coeffRef (2) += c * d; + ATb.coeffRef (3) += nx * d; + ATb.coeffRef (4) += ny * d; + ATb.coeffRef (5) += nz * d; + } + } - const double & nx = m00*src_nx + m01*src_ny + m02*src_nz; - const double & ny = m10*src_nx + m11*src_ny + m12*src_nz; - const double & nz = m20*src_nx + m21*src_ny + m22*src_nz; + double wsum = 0; + double wsx = 0; + double wsy = 0; + double wsz = 0; +//printf("%s::%i\n",__FUNCTION__,__LINE__); + if(use_depthedge){ +//printf("%s::%i\n",__FUNCTION__,__LINE__); + for(unsigned int co = 0; co < depthedge_count; co++){ +// printf("%s::%i\n",__FUNCTION__,__LINE__); + const double & src_x = depthedge_Xp_arr[3*co+0]; + const double & src_y = depthedge_Xp_arr[3*co+1]; + const double & src_z = depthedge_Xp_arr[3*co+2]; + + const double & dx = depthedge_Qp_arr[3*co+0]; + const double & dy = depthedge_Qp_arr[3*co+1]; + const double & dz = depthedge_Qp_arr[3*co+2]; + + const double & rw = depthedge_rangeW_arr[co]; + + const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; + const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; + const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; + + + const double diffX = dx-sx; + const double diffY = dy-sy; + const double diffZ = dz-sz; + + double probX = depthedge_func->getProb(rw*diffX); + double probY = depthedge_func->getProb(rw*diffY); + double probZ = depthedge_func->getProb(rw*diffZ); + double prob = probX*probY*probZ/(probX*probY*probZ + (1-probX)*(1-probY)*(1-probZ)); + +//printf("%5.5i -> %5.5f %5.5f %5.5f -> %5.5f %5.5f %5.5f , %5.5f %5.5f %5.5f -> prob: %5.5f\n",co,src_x,src_y,src_z,sx,sy,sz,dx,dy,dz,prob); + + if(prob < 0.000001){continue;} + +// if(visualizationLvl == 5){ +// pcl::PointXYZRGBNormal p; +// p.x = sx; +// p.y = sy; +// p.z = sz; +// p.b = 0; +// p.g = 255; +// p.r = 0; + +// pcl::PointXYZRGBNormal p1; +// p1.x = dx; +// p1.y = dy; +// p1.z = dz; +// p1.b = 255.0; +// p1.g = 0; +// p1.r = 0; +// dcloud->points.push_back(p1); + +//// char buf [1024]; +//// sprintf(buf,"line%i",scloud->points.size()); +//// viewer->addLine(p,p1,1.0-prob,prob,0,buf); +// } + + if(visualizationLvl == 5){ + +// printf("%5.5i -> %5.5f %5.5f %5.5f , %5.5f %5.5f %5.5f \n",co,sx,sy,sz,dx,dy,dz); + + pcl::PointXYZRGBNormal p; + p.x = sx; + p.y = sy; + p.z = sz; + p.b = 0; + p.g = 255; + p.r = 0; + scloud->points.push_back(p); + + pcl::PointXYZRGBNormal p1; + p1.x = dx; + p1.y = dy; + p1.z = dz; + p1.b = 255.0*prob; + p1.g = 255.0*prob; + p1.r = 255.0*prob; + dcloud->points.push_back(p1); + } - const double & angle = nx*dnx+ny*dny+nz*dnz; - //printf("%f\n",angle); - //if(angle < 0){exit(0);continue;} + //double weight = kpInfo*prob*rw*rw; - if(angle < 0){continue;} + double weight = depthedge_kpInfo*prob*rw*rw; + wsum += weight; - double di = rw*(nx*(sx-dx) + ny*(sy-dy) + nz*(sz-dz)); - //double weight = angle*angle*angle*angle*func->getProb(di)*rw*rw; - double prob = func->getProb(di); - double weight = planeInfo*prob*rw*rw; + wsx += weight * sx; + wsy += weight * sy; + wsz += weight * sz; - if(visualizationLvl == 5 && i == 0){ - pcl::PointXYZRGBNormal p; - p.x = sx; - p.y = sy; - p.z = sz; - p.b = 0; - p.g = 255; - p.r = 0; - scloud->points.push_back(p); + double wsxsx = weight * sx*sx; + double wsysy = weight * sy*sy; + double wszsz = weight * sz*sz; - pcl::PointXYZRGBNormal p1; - p1.x = dx; - p1.y = dy; - p1.z = dz; - p1.b = 255.0*prob; - p1.g = 255.0*prob; - p1.r = 255.0*prob; - dcloud->points.push_back(p1); - } + ATA.coeffRef (0) += wsysy + wszsz;//a0 * a0; + ATA.coeffRef (1) -= weight * sx*sy;//a0 * a1; + ATA.coeffRef (2) -= weight * sz*sx;//a0 * a2; - const double & a = nz*sy - ny*sz; - const double & b = nx*sz - nz*sx; - const double & c = ny*sx - nx*sy; - ATA.coeffRef (0) += weight * a * a; - ATA.coeffRef (1) += weight * a * b; - ATA.coeffRef (2) += weight * a * c; - ATA.coeffRef (3) += weight * a * nx; - ATA.coeffRef (4) += weight * a * ny; - ATA.coeffRef (5) += weight * a * nz; - ATA.coeffRef (7) += weight * b * b; - ATA.coeffRef (8) += weight * b * c; - ATA.coeffRef (9) += weight * b * nx; - ATA.coeffRef (10) += weight * b * ny; - ATA.coeffRef (11) += weight * b * nz; - ATA.coeffRef (14) += weight * c * c; - ATA.coeffRef (15) += weight * c * nx; - ATA.coeffRef (16) += weight * c * ny; - ATA.coeffRef (17) += weight * c * nz; - ATA.coeffRef (21) += weight * nx * nx; - ATA.coeffRef (22) += weight * nx * ny; - ATA.coeffRef (23) += weight * nx * nz; - ATA.coeffRef (28) += weight * ny * ny; - ATA.coeffRef (29) += weight * ny * nz; - ATA.coeffRef (35) += weight * nz * nz; + ATA.coeffRef (7) += wsxsx + wszsz;//a1 * a1; + ATA.coeffRef (8) -= weight * sy*sz;//a1 * a2; - const double & d = weight * (nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz); + ATA.coeffRef (14) += wsxsx + wsysy;//a2 * a2; - ATb.coeffRef (0) += a * d; - ATb.coeffRef (1) += b * d; - ATb.coeffRef (2) += c * d; - ATb.coeffRef (3) += nx * d; - ATb.coeffRef (4) += ny * d; - ATb.coeffRef (5) += nz * d; + ATb.coeffRef (0) += weight * (sy*diffZ -sz*diffY);//a0 * b; + ATb.coeffRef (1) += weight * (-sx*diffZ + sz*diffX);//a1 * b; + ATb.coeffRef (2) += weight * (sx*diffY -sy*diffX);//a2 * b; + ATb.coeffRef (3) += weight * diffX;//a3 * b; + ATb.coeffRef (4) += weight * diffY;//a4 * b; + ATb.coeffRef (5) += weight * diffZ;//a5 * b; + } } - double wsum = 0; - double wsx = 0; - double wsy = 0; - double wsz = 0; for(unsigned int co = 0; co < kp_count; co++){ const double & src_x = kp_Xp_arr[3*co+0]; const double & src_y = kp_Xp_arr[3*co+1]; @@ -1662,7 +1871,7 @@ std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vectorremoveAllPointClouds(); viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); //viewer->addPointCloudNormals(scloud,1,0.2,"scloudn"); @@ -2567,6 +2777,42 @@ if(false){ } +void MassRegistrationPPR2::showEdges(std::vector poses){ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + for(unsigned int i = 0; i < poses.size(); i++){ + Eigen::Matrix4d p = poses[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + int r,g,b; + r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); + g = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); + b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1); + + for(unsigned int c = 0; c < depthedge_nr_arraypoints[i]; c++){ + pcl::PointXYZRGBNormal p; + + float x = depthedge_arraypoints.at(i)[3*c+0]; + float y = depthedge_arraypoints.at(i)[3*c+1]; + float z = depthedge_arraypoints.at(i)[3*c+2]; + + + p.x = m00*x + m01*y + m02*z + m03; + p.y = m10*x + m11*y + m12*z + m13; + p.z = m20*x + m21*y + m22*z + m23; + p.b = r; + p.g = g; + p.r = b; + cloud->points.push_back(p); + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); +} + MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses){ if(visualizationLvl > 0){printf("start MassRegistrationPPR2::getTransforms(std::vector poses)\n");} @@ -2582,7 +2828,14 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector & X = points[i]; Eigen::Matrix & Xn = normals[i]; @@ -2618,14 +2874,18 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectorreset(); kpfunc->reset(); + depthedge_func->reset(); int imgcount = 0; char buf [1024]; - if(visualizationLvl > 0){ - std::vector Xv; - for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} - sprintf(buf,"image%5.5i.png",imgcount++); - show(Xv,false,std::string(buf),imgcount); + if(visualizationLvl == 1){ + if(use_depthedge){showEdges(poses);} + if(use_surface){ + std::vector Xv; + for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + sprintf(buf,"image%5.5i.png",imgcount++); + show(Xv,false,std::string(buf),imgcount); + } } rematch_time = 0; @@ -2646,10 +2906,29 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector timeout){break;} - if(visualizationLvl == 2){std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + //if(visualizationLvl == 2){if(use_depthedge){showEdges(poses);}std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount); showEdges(poses);} + if(visualizationLvl == 2){ + if(use_depthedge){showEdges(poses);} + if(use_surface){ + std::vector Xv; + for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + sprintf(buf,"image%5.5i.png",imgcount++); + show(Xv,false,std::string(buf),imgcount); + } + } for(int rematching=0; rematching < 10; ++rematching) { - if(visualizationLvl == 3){std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + //if(visualizationLvl == 3){if(use_depthedge){showEdges(poses);}std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + if(visualizationLvl == 3){ + if(use_depthedge){showEdges(poses);} + if(use_surface){ + std::vector Xv; + for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + sprintf(buf,"image%5.5i.png",imgcount++); + show(Xv,false,std::string(buf),imgcount); + } + } + std::vector poses1 = poses; double rematch_time_start = getTime(); @@ -2668,19 +2947,48 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + //if(visualizationLvl == 4){if(use_depthedge){showEdges(poses);}std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + if(visualizationLvl == 4){ + if(use_depthedge){showEdges(poses);} + if(use_surface){ + std::vector Xv; + for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + sprintf(buf,"image%5.5i.png",imgcount++); + show(Xv,false,std::string(buf),imgcount); + } + } + std::vector poses2b = poses; double residuals_time_start = getTime(); - Eigen::MatrixXd all_residuals = getAllResiduals(poses); + Eigen::MatrixXd all_residuals; + + if(use_surface){ + all_residuals = getAllResiduals(poses); + } + + Eigen::MatrixXd depthedge_all_residuals; + if(use_depthedge){ + depthedge_all_residuals = depthedge_getAllResiduals(poses); + } // Eigen::MatrixXd all_KPresiduals = getAllKpResiduals(poses); residuals_time += getTime()-residuals_time_start; double computeModel_time_start = getTime(); - func->computeModel(all_residuals); + + if(use_surface){ + func->computeModel(all_residuals); + } + + if(use_depthedge){ + depthedge_func->computeModel(depthedge_all_residuals); + } + + printf("surface reg: %5.5f noise: %5.5f\n",func->regularization,func->noiseval); + printf("edge: reg: %5.5f noise: %5.5f\n",depthedge_func->regularization,depthedge_func->noiseval); // kpfunc->computeModel(all_KPresiduals); - stopval = func->getNoise()*0.1; + //stopval = func->getNoise()*0.1; // printf("reg: %f noise:%f stopval: %f\n",func->regularization,func->noiseval,stopval); // double stopval1 = func->getNoise()*0.1; @@ -2704,9 +3012,14 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectorgetNoise(); - func->update(); + func->update(); double noise_after = func->getNoise(); - double ratio = noise_after/noise_before; + double ratio = noise_after/noise_before; + + double depthedge_noise_before = depthedge_func->getNoise(); + depthedge_func->update(); + double depthedge_noise_after = depthedge_func->getNoise(); + double depthedge_ratio = depthedge_noise_after/depthedge_noise_before; double kpnoise_before = kpfunc->getNoise(); kpfunc->update(); @@ -2736,6 +3049,8 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector 0){ + showEdges(poses); + std::vector Xv; for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} sprintf(buf,"image%5.5i.png",imgcount++); From a7414ed047c611aa488d2fd29ed7cf857c370f26 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 2 Aug 2016 06:53:04 +0200 Subject: [PATCH 044/255] removed not needed dep, made registartion faster by not rematching non-overlapping regions --- .../quasimodo_brain/src/test_segment.cpp | 6 +- quasimodo/quasimodo_models/CMakeLists.txt | 4 +- .../registration/MassRegistrationPPR2.h | 6 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 17 ++- .../src/registration/MassRegistrationPPR2.cpp | 137 ++++++++++++++++-- 5 files changed, 145 insertions(+), 25 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index 5b4a338d..4f4c5a4e 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -329,7 +329,7 @@ reglib::Model * load2(std::string sweep_xml){ SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS - cam->fx = 532.158936; + cam->fx = 532.158936; cam->fy = 533.819214; cam->cx = 310.514310; cam->cy = 236.842039; @@ -709,11 +709,11 @@ int main(int argc, char** argv){ mu->massreg_timeout = 60*4; mu->viewer = viewer; - reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.0); + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); bgmassreg->timeout = 1200; bgmassreg->viewer = viewer; bgmassreg->use_surface = true; - bgmassreg->use_depthedge = true; + bgmassreg->use_depthedge = false; bgmassreg->visualizationLvl = 1; bgmassreg->maskstep = 10; bgmassreg->nomaskstep = 10; diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 9891b230..9c8c56f6 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -52,8 +52,6 @@ include_directories(/usr/local/include/) FIND_PACKAGE(Ceres REQUIRED) INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) -add_subdirectory (densecrf) - add_library(quasimodo_weightlib src/weightfunctions/DistanceWeightFunction2.cpp src/weightfunctions/DistanceWeightFunction2PPR.cpp src/weightfunctions/DistanceWeightFunction2PPR2.cpp) target_link_libraries(quasimodo_weightlib ceres glog ${catkin_LIBRARIES}) @@ -76,7 +74,7 @@ add_library(quasimodo_Mesher src/mesher/Mesh.cpp) target_link_libraries(quasimodo_Mesher quasimodo_model ${catkin_LIBRARIES}) add_library(quasimodo_ModelUpdater src/modelupdater/ModelUpdater.cpp src/modelupdater/ModelUpdaterBasic.cpp src/modelupdater/ModelUpdaterBasicFuse.cpp) -target_link_libraries(quasimodo_ModelUpdater densecrf quasimodo_Mesher quasimodo_model quasimodo_reglib quasimodo_massreglib ${catkin_LIBRARIES}) +target_link_libraries(quasimodo_ModelUpdater quasimodo_Mesher quasimodo_model quasimodo_reglib quasimodo_massreglib ${catkin_LIBRARIES}) diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h index 4630471c..55e0bf02 100644 --- a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h +++ b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h @@ -101,8 +101,10 @@ namespace reglib bool use_depthedge; - std::vector< int > depthedge_nr_arraypoints; - std::vector< double * > depthedge_arraypoints; + int depthedge_nr_neighbours; + std::vector< int > depthedge_nr_arraypoints; + std::vector< double * > depthedge_arraypoints; + std::vector< int * > depthedge_neighbours; std::vector< double * > depthedge_arrayinformations; std::vector< Tree3d * > depthedge_trees3d; std::vector< ArrayData3D * > depthedge_a3dv; diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 66e4ddac..9011c343 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -362,7 +362,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt for(unsigned int i = 0; i < label_indices[1].indices.size(); i++){ int ind = label_indices[1].indices[i]; - depthedgesdata[ind] = 255; + depthedgesdata[ind] = 1; // out.data[3*ind+0] =255; // out.data[3*ind+1] =0; // out.data[3*ind+2] =255; @@ -391,6 +391,21 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt //show(true); } + +// show(true); +// cv::Mat rgbclone = rgb.clone(); +// for(int w = 0; w < width; w++){ +// for(int h = 0; h < height;h++){ +// int ind = h*width+w; +// if(depthedgesdata[ind] == 255){ +// cv::circle(rgbclone, cv::Point(w,h), 3, cv::Scalar(0,255,0)); +// cv::imshow( "rgbclone", rgbclone ); +// cv::waitKey(0); +// } +// } +// } + + } RGBDFrame::~RGBDFrame(){} diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 9c0b0707..15301215 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -77,6 +77,8 @@ MassRegistrationPPR2::MassRegistrationPPR2(double startreg, bool visualize){ depthedge_Qp_arr = new double[3*maxcount+0]; depthedge_Xp_arr = new double[3*maxcount+0]; depthedge_rangeW_arr = new double[maxcount+0]; + + depthedge_nr_neighbours = 4; } MassRegistrationPPR2::~MassRegistrationPPR2(){ @@ -524,7 +526,7 @@ void MassRegistrationPPR2::setData(std::vector frames_,std::vector & matchid, std::vector & matchdist, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ +double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, int nr_ap, double * ap, std::vector & matchid, std::vector & matchdist, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); @@ -532,6 +534,10 @@ void matchframes(Eigen::Affine3d rp, int nr_ap, double * ap, std::vector & matchid.resize(nr_ap); matchdist.resize(nr_ap); + double threshold = pow(10*f->getNoise(),2); + double good = 0; + double bad = 0; + #pragma omp parallel for num_threads(8) for(unsigned int k = 0; k < nr_ap; ++k) { int prev = matchid[k]; @@ -549,12 +555,72 @@ void matchframes(Eigen::Affine3d rp, int nr_ap, double * ap, std::vector & resultSet.init(&ret_index, &out_dist_sqr ); t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + if(out_dist_sqr < threshold){ + good++; + }else{ + bad++; + } + int current = ret_index; new_good_rematches += prev != current; new_total_rematches++; matchid[k] = current; matchdist[k] = out_dist_sqr; } + + return good/(good+bad+0.001); +} + +double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, int nr_ap, double * ap, std::vector & matchid, std::vector & matchdist, double * apj, int nr_dn, int * dn, double & new_good_rematches, double & new_total_rematches){ + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + + matchid.resize(nr_ap); + matchdist.resize(nr_ap); + + double threshold = pow(10*f->getNoise(),2); + double good = 0; + double bad = 0; + + #pragma omp parallel for num_threads(8) + for(unsigned int k = 0; k < nr_ap; ++k) { + int prev = matchid[k]; + double qp [3]; + const double & src_x = ap[3*k+0]; + const double & src_y = ap[3*k+1]; + const double & src_z = ap[3*k+2]; + + const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; + const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; + const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; + for(unsigned int m = 0; m < nr_dn; ++m) { + int id = dn[nr_dn*k+m]; + const double & dx =apj[3*id+0]-sx; + const double & dy =apj[3*id+1]-sy; + const double & dz =apj[3*id+2]-sz; + const double sqr_dist = dx*dx+dy*dy+dz*dz; + } + +// size_t ret_index; double out_dist_sqr; +// nanoflann::KNNResultSet resultSet(1); +// resultSet.init(&ret_index, &out_dist_sqr ); +// t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + +// if(out_dist_sqr < threshold){ +// good++; +// }else{ +// bad++; +// } + +// int current = ret_index; +// new_good_rematches += prev != current; +// new_total_rematches++; +// matchid[k] = current; +// matchdist[k] = out_dist_sqr; + } + + return good/(good+bad+0.001); } void MassRegistrationPPR2::rematch(std::vector poses, std::vector prev_poses, bool first){ @@ -566,17 +632,26 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect int rmt = 2; if(rmt==2){ - int ignores = 0; + int overlapping = 0; + + int work_done = 0; + int ignores_motion = 0; + int ignores_overlap = 0; for(unsigned int i = 0; i < nr_frames; i++){ if(!is_ok[i]){continue;} nr_matches[i] = 0; + for(unsigned int j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} if(i == j){continue;} + if((matchscores[i][j] <= 0.001) && (matchscores[j][i] <= 0.001) && (rand()%nr_frames > 1)){ + ignores_overlap++; + continue; + } + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); - if(!first){ + if(false && !first){ Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); - Eigen::Affine3d diff = prev_rp.inverse()*rp; double change_trans = 0; @@ -591,18 +666,30 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect } change_trans += sqrt(dt); - //if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;} + if(change_trans < 1.0*stopval && change_rot < 1.0*stopval){ignores_motion++;continue;} } + + + double ratiosum = 0; if(use_depthedge){ - matchframes(rp, depthedge_nr_arraypoints[i], depthedge_arraypoints[i], depthedge_matchids[i][j], depthedge_matchdists[i][j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); - } + ratiosum += matchframes(depthedge_func,rp, depthedge_nr_arraypoints[i], depthedge_arraypoints[i], depthedge_matchids[i][j], depthedge_matchdists[i][j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); + } if(use_surface){ - matchframes(rp, nr_arraypoints[i], arraypoints[i], matchids[i][j], matchdists[i][j],trees3d[j], new_good_rematches,new_total_rematches); + ratiosum += matchframes(func,rp, nr_arraypoints[i], arraypoints[i], matchids[i][j], matchdists[i][j],trees3d[j], new_good_rematches,new_total_rematches); } + + matchscores[i][j] = ratiosum; + printf("%i %i -> %5.5f\n",i,j,ratiosum); + overlapping += ratiosum > 0; + work_done++; + } } - //printf("ignores: %i rematches: %i ratio of good rematches: %4.4f\n",ignores,nr_frames*(nr_frames-1)-ignores,new_good_rematches/new_total_rematches); + + printf("ignores_overlap: %i/%i -> %5.5f\n",ignores_overlap,nr_frames*nr_frames - nr_frames, double(ignores_overlap)/double(nr_frames*nr_frames - nr_frames)); + printf("ignores_motion: %i/%i -> %5.5f\n",ignores_motion,nr_frames*nr_frames - nr_frames, double(ignores_motion)/double(nr_frames*nr_frames - nr_frames)); + printf("total work done: %i/%i -> %5.5f\n",work_done,nr_frames*nr_frames - nr_frames, double(work_done)/double(nr_frames*nr_frames - nr_frames)); } if(rmt==1){ @@ -1074,6 +1161,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ depthedge_matchids.push_back( std::vector< std::vector >() ); depthedge_matchdists.push_back( std::vector< std::vector >() ); + depthedge_neighbours.push_back(0); depthedge_nr_arraypoints.push_back(0); depthedge_arraypoints.push_back(0); depthedge_arrayinformations.push_back(0); @@ -1204,8 +1292,8 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ int depthedge_count = 0; - for(unsigned int w = 0; w < width; w+=2){ - for(unsigned int h = 0; h < height; h+=2){ + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height; h++){ int ind = h*width+w; if(maskvec[ind] && edgedata[ind] == 255){ float z = idepth*float(depthdata[ind]); @@ -1218,6 +1306,8 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ if(depthedge_count > 10){ double * depthedge_ap = new double[3*depthedge_count]; double * depthedge_ai = new double[3*depthedge_count]; + int * dn = new int[depthedge_nr_neighbours*depthedge_count]; + depthedge_neighbours[i] = dn; depthedge_nr_arraypoints[i] = depthedge_count; depthedge_arraypoints[i] = depthedge_ap; depthedge_arrayinformations[i] = depthedge_ai; @@ -1246,6 +1336,21 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ depthedge_a3dv[i] = depthedge_a3d; depthedge_trees3d[i] = new Tree3d(3, *depthedge_a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); depthedge_trees3d[i]->buildIndex(); + Tree3d * t3d = depthedge_trees3d[i]; + + + const int nrdn = depthedge_nr_neighbours+1; + #pragma omp parallel for num_threads(8) + for(int c = 0; c < depthedge_count; c++){ + size_t ret_index[nrdn]; + double out_dist_sqr[nrdn]; + nanoflann::KNNResultSet resultSet(nrdn); + resultSet.init(ret_index, out_dist_sqr ); + t3d->findNeighbors(resultSet, depthedge_ap+3*c, nanoflann::SearchParams(10)); + for(int k = 0; k < depthedge_nr_neighbours; k++){ + dn[depthedge_nr_neighbours*c + k] = ret_index[k+1]; + } + } } printf("total load time: %5.5f\n",getTime()-total_load_time_start); @@ -2780,6 +2885,7 @@ if(false){ void MassRegistrationPPR2::showEdges(std::vector poses){ pcl::PointCloud::Ptr cloud (new pcl::PointCloud); for(unsigned int i = 0; i < poses.size(); i++){ + printf("%i/%i -> %i\n",i,poses.size(),depthedge_nr_arraypoints[i]); Eigen::Matrix4d p = poses[i]; float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); @@ -2807,10 +2913,9 @@ void MassRegistrationPPR2::showEdges(std::vector poses){ cloud->points.push_back(p); } } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); } MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses){ @@ -2833,7 +2938,7 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector Date: Tue, 2 Aug 2016 17:41:12 +0200 Subject: [PATCH 045/255] edge pickng --- .../quasimodo_brain/src/test_segment.cpp | 2 +- .../registration/MassRegistrationPPR2.h | 4 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 193 ++++++++++++++++-- .../src/registration/MassRegistrationPPR2.cpp | 99 ++++++--- 4 files changed, 251 insertions(+), 47 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index 4f4c5a4e..38bd2e9a 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -713,7 +713,7 @@ int main(int argc, char** argv){ bgmassreg->timeout = 1200; bgmassreg->viewer = viewer; bgmassreg->use_surface = true; - bgmassreg->use_depthedge = false; + bgmassreg->use_depthedge = true; bgmassreg->visualizationLvl = 1; bgmassreg->maskstep = 10; bgmassreg->nomaskstep = 10; diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h index 55e0bf02..67d46d12 100644 --- a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h +++ b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h @@ -101,10 +101,10 @@ namespace reglib bool use_depthedge; - int depthedge_nr_neighbours; + //int depthedge_nr_neighbours; std::vector< int > depthedge_nr_arraypoints; std::vector< double * > depthedge_arraypoints; - std::vector< int * > depthedge_neighbours; + //std::vector< int * > depthedge_neighbours; std::vector< double * > depthedge_arrayinformations; std::vector< Tree3d * > depthedge_trees3d; std::vector< ArrayData3D * > depthedge_a3dv; diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 9011c343..b21224fd 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -171,12 +171,14 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } */ + + //printf("%s LINE:%i\n",__FILE__,__LINE__); if(compute_normals){ normals.create(height,width,CV_32FC3); float * normalsdata = (float *)normals.data; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); pcl::PointCloud::Ptr normals_cloud (new pcl::PointCloud); cloud->width = width; cloud->height = height; @@ -392,19 +394,184 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt //show(true); } -// show(true); -// cv::Mat rgbclone = rgb.clone(); -// for(int w = 0; w < width; w++){ -// for(int h = 0; h < height;h++){ -// int ind = h*width+w; -// if(depthedgesdata[ind] == 255){ -// cv::circle(rgbclone, cv::Point(w,h), 3, cv::Scalar(0,255,0)); -// cv::imshow( "rgbclone", rgbclone ); -// cv::waitKey(0); -// } -// } -// } + show(false); + + int * last = new int[width*height]; + for(int i = 0; i < width*height; i++){last[i] = 0;} + int current = 1; + + boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0, 0, 0); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + for(int w = 0; w < width; w+=3){ + for(int h = 0; h < height;h+=3){ + int ind = h*width+w; + if(depthedgesdata[ind] == 255 && depthdata[ind] != 0){ + double xx = 0.001; + double xy = 0; + double xz = 0; + double yy = xx; + double yz = 0; + double zz = xx; + double sum = 0.001; + + double z = idepth*double(depthdata[ind]); + double x = (double(w) - cx) * z * ifx; + double y = (double(h) - cy) * z * ify; + +// for(int ww = 0; ww < width; ww++){ +// for(int hh = 0; hh < height;hh++){ +// int ind = hh*width+ww; +// pcl::PointXYZRGBA & p = cloud->points[ind]; +// p.b = 0; +// p.g = 255; +// p.r = 0; +// } +// } + + + + + std::vector todolistw; + todolistw.push_back(w); + std::vector todolisth; + todolisth.push_back(h); + current++; + last[ind] = current; + cv::Mat rgbclone = rgb.clone(); + unsigned char * rgbclonedata = (unsigned char *)rgbclone.data; + + + cv::circle(rgbclone, cv::Point(w,h), 3, cv::Scalar(0,255,0)); + cv::imshow( "rgbclone", rgbclone ); + + int tmp; + for(int go = 0; go < 20 && go < todolistw.size(); go++){ + int cw = todolistw[go]; + int ch = todolisth[go]; + int ind = ch*width+cw; + + + double z2 = idepth*double(depthdata[ind]); + if(z2 > 0){ + double x2 = (double(cw) - cx) * z2 * ifx; + double y2 = (double(ch) - cy) * z2 * ify; + + double dx = x-x2; + double dy = y-y2; + double dz = z-z2; + + xx += dx*dx; + xy += dx*dy; + xz += dx*dz; + + yy += dy*dy; + yz += dy*dz; + + zz += dz*dz; + sum++; + } + + //double z = idepth*double(depthdata[ind]); + rgbclonedata[3*ind+0] = 0; + rgbclonedata[3*ind+1] = 0; + rgbclonedata[3*ind+2] = 255; + +// cloud->points[ind].r = 255; +// cloud->points[ind].g = 0; +// cloud->points[ind].b = 0; + + int startw = std::max(int(0),cw-1); + int stopw = std::min(int(width-1),cw+1); + + int starth = std::max(int(0),ch-1); + int stoph = std::min(int(height-1),ch+1); + + for(int tw = startw; tw <= stopw; tw++){ + for(int th = starth; th <= stoph; th++){ + int ind2 = th*width+tw; + if(depthedgesdata[ind2] == 255 && last[ind2] != current){ + todolistw.push_back(tw); + todolisth.push_back(th); + last[ind2] = current; + } + } + } + + } + + //cv::circle(rgbclone, cv::Point(w,h), 3, cv::Scalar(0,255,0)); + + + cv::imshow( "rgbclone", rgbclone ); + cv::waitKey(0); + + + pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); + cloud2->width = width; + cloud2->height = height; + cloud2->points.resize(width*height); + //cloud->dense = false; + + unsigned short * depthdata = (unsigned short *)depth.data; + unsigned char * rgbdata = (unsigned char *)rgb.data; + for(int w3 = 0; w3 < width; w3++){ + for(int h3 = 0; h3 < height;h3++){ + int ind3 = h3*width+w3; + pcl::PointXYZRGBA & p = cloud2->points[ind3]; + p.b = rgbdata[3*ind3+0];//255; + p.g = rgbdata[3*ind3+1];//255; + p.r = rgbdata[3*ind3+2];//255; + double z3 = 1;//idepth*double(depthdata[ind3]); + + if(w3%30 == 0 && h3%30 == 0){printf("%i %i -> %f\n",w3,h3,z3);} + if(z3 > 0){ + p.x = (double(w3) - cx) * z3 * ifx; + p.y = (double(h3) - cy) * z3 * ify; + p.z = z3; + }else{ + p.x = NAN; + p.y = NAN; + p.z = NAN; + } + } + } + + for(int go = 0; go < todolistw.size(); go++){ + int cw = todolistw[go]; + int ch = todolisth[go]; + int ind3 = ch*width+cw; + + pcl::PointXYZRGBA & p = cloud2->points[ind3]; + p.b = 0; + p.g = 0; + p.r = 255; + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud"); + viewer->spin(); + + for(int w3 = 0; w3 < width; w3++){ + for(int h3 = 0; h3 < height;h3++){ + int ind3 = h3*width+w3; + pcl::PointXYZRGBA & p = cloud2->points[ind3]; + + double z3 = 1;//idepth*double(depthdata[ind3]); + p.b = rgbdata[3*ind3+0];//255; + p.g = rgbdata[3*ind3+1];//255; + p.r = rgbdata[3*ind3+2];//255; + } + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud"); + viewer->spin(); + } + } + } } diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 15301215..0c2e2111 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -78,7 +78,7 @@ MassRegistrationPPR2::MassRegistrationPPR2(double startreg, bool visualize){ depthedge_Xp_arr = new double[3*maxcount+0]; depthedge_rangeW_arr = new double[maxcount+0]; - depthedge_nr_neighbours = 4; + //depthedge_nr_neighbours = 10; } MassRegistrationPPR2::~MassRegistrationPPR2(){ @@ -571,7 +571,7 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, int nr_a return good/(good+bad+0.001); } -double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, int nr_ap, double * ap, std::vector & matchid, std::vector & matchdist, double * apj, int nr_dn, int * dn, double & new_good_rematches, double & new_total_rematches){ +double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, int nr_ap, double * ap, std::vector & matchid, std::vector & matchdist, double * apj, int nr_dn, int * dn, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); @@ -585,8 +585,6 @@ double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, i #pragma omp parallel for num_threads(8) for(unsigned int k = 0; k < nr_ap; ++k) { - int prev = matchid[k]; - double qp [3]; const double & src_x = ap[3*k+0]; const double & src_y = ap[3*k+1]; const double & src_z = ap[3*k+2]; @@ -594,18 +592,53 @@ double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, i const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; - for(unsigned int m = 0; m < nr_dn; ++m) { - int id = dn[nr_dn*k+m]; - const double & dx =apj[3*id+0]-sx; - const double & dy =apj[3*id+1]-sy; - const double & dz =apj[3*id+2]-sz; - const double sqr_dist = dx*dx+dy*dy+dz*dz; - } -// size_t ret_index; double out_dist_sqr; -// nanoflann::KNNResultSet resultSet(1); -// resultSet.init(&ret_index, &out_dist_sqr ); -// t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + int prev = matchid[k]; + const double & dx1 =apj[3*prev+0]-sx; + const double & dy1 =apj[3*prev+1]-sy; + const double & dz1 =apj[3*prev+2]-sz; + + double before = dx1*dx1+dy1*dy1+dz1*dz1;; + double mindist = before; + + printf("point %4.4i before: %5.5f -> ",k,before); + while(true){ + int next = prev; + for(unsigned int m = 0; m < nr_dn; ++m) { + int id = dn[nr_dn*prev+m]; + const double & dx =apj[3*id+0]-sx; + const double & dy =apj[3*id+1]-sy; + const double & dz =apj[3*id+2]-sz; + const double sqr_dist = dx*dx+dy*dy+dz*dz; + if(sqr_dist < mindist){ + mindist = sqr_dist; + next = id; + } + } + printf("%5.5f ",mindist); + if(next == prev){break;} + prev = next; + } + + double qp [3]; + qp[0] = sx; + qp[1] = sy; + qp[2] = sz; + size_t ret_index; double out_dist_sqr; + nanoflann::KNNResultSet resultSet(1); + resultSet.init(&ret_index, &out_dist_sqr ); + t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + + const double & dx2 =apj[3*ret_index+0]-sx; + const double & dy2 =apj[3*ret_index+1]-sy; + const double & dz2 =apj[3*ret_index+2]-sz; + + double gt = dx2*dx2+dy2*dy2+dz2*dz2; + + //printf("point %4.4i before: %5.5f after: %5.5f gt: %5.5f\n",k,before,mindist,out_dist_sqr); + printf(" gt: %5.5f gt: %5.5f\n",out_dist_sqr,gt); + + // if(out_dist_sqr < threshold){ // good++; @@ -620,7 +653,7 @@ double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, i // matchdist[k] = out_dist_sqr; } - return good/(good+bad+0.001); + return 1; } void MassRegistrationPPR2::rematch(std::vector poses, std::vector prev_poses, bool first){ @@ -673,6 +706,10 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect double ratiosum = 0; if(use_depthedge){ +// if(rand()%100 == 0){ +// update_matchframes(depthedge_func,rp,depthedge_nr_arraypoints[i],depthedge_arraypoints[i],depthedge_matchids[i][j],depthedge_matchdists[i][j],depthedge_arraypoints[j],depthedge_nr_neighbours, depthedge_neighbours[j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); +// exit(0); +// } ratiosum += matchframes(depthedge_func,rp, depthedge_nr_arraypoints[i], depthedge_arraypoints[i], depthedge_matchids[i][j], depthedge_matchdists[i][j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); } if(use_surface){ @@ -1161,7 +1198,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ depthedge_matchids.push_back( std::vector< std::vector >() ); depthedge_matchdists.push_back( std::vector< std::vector >() ); - depthedge_neighbours.push_back(0); + //depthedge_neighbours.push_back(0); depthedge_nr_arraypoints.push_back(0); depthedge_arraypoints.push_back(0); depthedge_arrayinformations.push_back(0); @@ -1306,8 +1343,8 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ if(depthedge_count > 10){ double * depthedge_ap = new double[3*depthedge_count]; double * depthedge_ai = new double[3*depthedge_count]; - int * dn = new int[depthedge_nr_neighbours*depthedge_count]; - depthedge_neighbours[i] = dn; + //int * dn = new int[depthedge_nr_neighbours*depthedge_count]; + //depthedge_neighbours[i] = dn; depthedge_nr_arraypoints[i] = depthedge_count; depthedge_arraypoints[i] = depthedge_ap; depthedge_arrayinformations[i] = depthedge_ai; @@ -1339,18 +1376,18 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ Tree3d * t3d = depthedge_trees3d[i]; - const int nrdn = depthedge_nr_neighbours+1; - #pragma omp parallel for num_threads(8) - for(int c = 0; c < depthedge_count; c++){ - size_t ret_index[nrdn]; - double out_dist_sqr[nrdn]; - nanoflann::KNNResultSet resultSet(nrdn); - resultSet.init(ret_index, out_dist_sqr ); - t3d->findNeighbors(resultSet, depthedge_ap+3*c, nanoflann::SearchParams(10)); - for(int k = 0; k < depthedge_nr_neighbours; k++){ - dn[depthedge_nr_neighbours*c + k] = ret_index[k+1]; - } - } +// const int nrdn = depthedge_nr_neighbours+1; +// #pragma omp parallel for num_threads(8) +// for(int c = 0; c < depthedge_count; c++){ +// size_t ret_index[nrdn]; +// double out_dist_sqr[nrdn]; +// nanoflann::KNNResultSet resultSet(nrdn); +// resultSet.init(ret_index, out_dist_sqr ); +// t3d->findNeighbors(resultSet, depthedge_ap+3*c, nanoflann::SearchParams(10)); +// for(int k = 0; k < depthedge_nr_neighbours; k++){ +// dn[depthedge_nr_neighbours*c + k] = ret_index[k+1]; +// } +// } } printf("total load time: %5.5f\n",getTime()-total_load_time_start); From 898d4d277f583bbe3a845f987d50e6912052edb3 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 4 Aug 2016 06:53:51 +0200 Subject: [PATCH 046/255] line dir extraction --- .../quasimodo_models/src/core/RGBDFrame.cpp | 96 ++++++++++++------- 1 file changed, 63 insertions(+), 33 deletions(-) diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index b21224fd..8df17a2b 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -381,7 +381,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt for(unsigned int i = 0; i < label_indices[4].indices.size(); i++){ int ind = label_indices[4].indices[i]; - depthedgesdata[ind] = 255; + depthedgesdata[ind] = 255; // out.data[3*ind+0] =0; // out.data[3*ind+1] =255; // out.data[3*ind+2] =0; @@ -408,7 +408,9 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt for(int w = 0; w < width; w+=3){ for(int h = 0; h < height;h+=3){ int ind = h*width+w; - if(depthedgesdata[ind] == 255 && depthdata[ind] != 0){ + + int edgetype = depthedgesdata[ind]; + if(edgetype != 0 && depthdata[ind] != 0){ double xx = 0.001; double xy = 0; double xz = 0; @@ -417,6 +419,12 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt double zz = xx; double sum = 0.001; + double ww = 0.0; + double wh = 0; + double hh = 0; + double sum2 = 0.0; + + double z = idepth*double(depthdata[ind]); double x = (double(w) - cx) * z * ifx; double y = (double(h) - cy) * z * ify; @@ -442,18 +450,36 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt last[ind] = current; cv::Mat rgbclone = rgb.clone(); - unsigned char * rgbclonedata = (unsigned char *)rgbclone.data; + unsigned char * rgbclonedata = (unsigned char *)rgbclone.data; cv::circle(rgbclone, cv::Point(w,h), 3, cv::Scalar(0,255,0)); cv::imshow( "rgbclone", rgbclone ); int tmp; - for(int go = 0; go < 20 && go < todolistw.size(); go++){ + for(int go = 0; go < 40 && go < todolistw.size(); go++){ int cw = todolistw[go]; int ch = todolisth[go]; int ind = ch*width+cw; + int dw = cw-w; + int dh = ch-h; + ww+=dw*dw; + wh+=dw*dh; + hh+=dh*dh; + sum2++; + + Eigen::Matrix2d mimg; + mimg(0,0) = ww/sum2; + mimg(0,1) = wh/sum2; + mimg(1,0) = wh/sum2; + mimg(1,1) = hh/sum2; + + Eigen::EigenSolver es(mimg); + cout << "The mimg are:" << endl << mimg << endl; + cout << "The eigenvalues of mimg are:" << endl << es.eigenvalues() << endl; + cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl; + double z2 = idepth*double(depthdata[ind]); if(z2 > 0){ @@ -493,7 +519,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt for(int tw = startw; tw <= stopw; tw++){ for(int th = starth; th <= stoph; th++){ int ind2 = th*width+tw; - if(depthedgesdata[ind2] == 255 && last[ind2] != current){ + if(depthedgesdata[ind2] == edgetype && last[ind2] != current){ todolistw.push_back(tw); todolisth.push_back(th); last[ind2] = current; @@ -503,11 +529,25 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } + ww /= sum2; + wh /= sum2; + hh /= sum2; + Eigen::Matrix2d mimg; + mimg(0,0) = ww; + mimg(0,1) = wh; + mimg(1,0) = wh; + mimg(1,1) = hh; + + Eigen::EigenSolver es(mimg); + cout << "The mimg are:" << endl << mimg << endl; + cout << "The eigenvalues of mimg are:" << endl << es.eigenvalues() << endl; + cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl; + //cv::circle(rgbclone, cv::Point(w,h), 3, cv::Scalar(0,255,0)); - cv::imshow( "rgbclone", rgbclone ); - cv::waitKey(0); +// cv::imshow( "rgbclone", rgbclone ); +// cv::waitKey(0); pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); @@ -525,18 +565,6 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt p.b = rgbdata[3*ind3+0];//255; p.g = rgbdata[3*ind3+1];//255; p.r = rgbdata[3*ind3+2];//255; - double z3 = 1;//idepth*double(depthdata[ind3]); - - if(w3%30 == 0 && h3%30 == 0){printf("%i %i -> %f\n",w3,h3,z3);} - if(z3 > 0){ - p.x = (double(w3) - cx) * z3 * ifx; - p.y = (double(h3) - cy) * z3 * ify; - p.z = z3; - }else{ - p.x = NAN; - p.y = NAN; - p.z = NAN; - } } } @@ -551,21 +579,23 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt p.r = 255; } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud"); - viewer->spin(); + for(int w3 = 0; w3 < width; w3++){ + for(int h3 = 0; h3 < height;h3++){ + int ind3 = h3*width+w3; + pcl::PointXYZRGBA & p = cloud2->points[ind3]; + double z3 = idepth*double(depthdata[ind3]); + if(z3 > 0){ + p.x = (double(w3) - cx) * z3 * ifx; + p.y = (double(h3) - cy) * z3 * ify; + p.z = z3; + }else{ + p.x = 0; + p.y = 0; + p.z = 0; + } + } + } - for(int w3 = 0; w3 < width; w3++){ - for(int h3 = 0; h3 < height;h3++){ - int ind3 = h3*width+w3; - pcl::PointXYZRGBA & p = cloud2->points[ind3]; - - double z3 = 1;//idepth*double(depthdata[ind3]); - p.b = rgbdata[3*ind3+0];//255; - p.g = rgbdata[3*ind3+1];//255; - p.r = rgbdata[3*ind3+2];//255; - } - } viewer->removeAllPointClouds(); viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud"); viewer->spin(); From 061d81333efb9f2e14a04f9d280cabfa51879033 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 4 Aug 2016 17:26:04 +0200 Subject: [PATCH 047/255] working on coherent segmentation between frames --- .../quasimodo_brain/src/test_segment.cpp | 8 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 72 +++-- .../src/modelupdater/ModelUpdater.cpp | 260 +++++++++++++++++- 3 files changed, 312 insertions(+), 28 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index 38bd2e9a..66c42520 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -713,10 +713,10 @@ int main(int argc, char** argv){ bgmassreg->timeout = 1200; bgmassreg->viewer = viewer; bgmassreg->use_surface = true; - bgmassreg->use_depthedge = true; - bgmassreg->visualizationLvl = 1; - bgmassreg->maskstep = 10; - bgmassreg->nomaskstep = 10; + bgmassreg->use_depthedge = false; + bgmassreg->visualizationLvl = 0; + bgmassreg->maskstep = 5; + bgmassreg->nomaskstep = 5; bgmassreg->nomask = true; bgmassreg->stopval = 0.0005; bgmassreg->setData(models.front()->frames,models.front()->modelmasks); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 8df17a2b..d8b39a6b 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -364,7 +364,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt for(unsigned int i = 0; i < label_indices[1].indices.size(); i++){ int ind = label_indices[1].indices[i]; - depthedgesdata[ind] = 1; + //depthedgesdata[ind] = 1; // out.data[3*ind+0] =255; // out.data[3*ind+1] =0; // out.data[3*ind+2] =255; @@ -393,7 +393,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt //show(true); } - +if(false){ show(false); int * last = new int[width*height]; @@ -405,8 +405,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt viewer->addCoordinateSystem (1.0); viewer->initCameraParameters (); - for(int w = 0; w < width; w+=3){ - for(int h = 0; h < height;h+=3){ + for(int w = 0; w < width; w+=5){ + for(int h = 0; h < height;h+=5){ int ind = h*width+w; int edgetype = depthedgesdata[ind]; @@ -419,10 +419,10 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt double zz = xx; double sum = 0.001; - double ww = 0.0; + double ww = 4.0; double wh = 0; - double hh = 0; - double sum2 = 0.0; + double hh = 4.0; + double sum2 = 4.0; double z = idepth*double(depthdata[ind]); @@ -453,11 +453,14 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt unsigned char * rgbclonedata = (unsigned char *)rgbclone.data; - cv::circle(rgbclone, cv::Point(w,h), 3, cv::Scalar(0,255,0)); + cv::circle(rgbclone, cv::Point(w,h), 2, cv::Scalar(0,255,0)); cv::imshow( "rgbclone", rgbclone ); + + printf("ratio = ["); + double best_ratio = 0; int tmp; - for(int go = 0; go < 40 && go < todolistw.size(); go++){ + for(int go = 0; go < 100000 && go < todolistw.size(); go++){ int cw = todolistw[go]; int ch = todolisth[go]; int ind = ch*width+cw; @@ -475,10 +478,43 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt mimg(1,0) = wh/sum2; mimg(1,1) = hh/sum2; - Eigen::EigenSolver es(mimg); - cout << "The mimg are:" << endl << mimg << endl; - cout << "The eigenvalues of mimg are:" << endl << es.eigenvalues() << endl; - cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl; + Eigen::EigenSolver es(mimg); + double e1 = (es.eigenvalues())(0).real(); + double e2 = (es.eigenvalues())(1).real(); + double ratio = 1; + if(e1 > e2){ratio = e1/e2;} + if(e2 > e1){ratio = e2/e1;} + printf("%5.5f ",ratio); + + if(ratio < best_ratio){ + ww-=dw*dw; + wh-=dw*dh; + hh-=dh*dh; + sum2--; + //cv::circle(rgbclone, cv::Point(cw,ch), 2, cv::Scalar(255,0,255)); + continue; + } + + best_ratio = ratio; +// cout << "The mimg are:" << endl << mimg << endl; +// cout << "The eigenvalues of mimg are:" << endl << es.eigenvalues() << endl; +// printf("%f %f\n",e1,e2); +// cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl; + +// B2=(-cov[0][0]-cov[1][1])*(-cov[0][0]-cov[1][1]); +// c= (4*cov[0][0]*cov[1][1])-(4*cov[0][1]*cov[1][0]); + + +// sq= sqrt(B2-c); + + +// B=(cov[0][0])+(cov[1][1]); + +// r1=(B+sq)(.5); +// r2=(B-sq)(.5); + + +// printf("\nThe eigenvalues are %f and %f", r1, r2); double z2 = idepth*double(depthdata[ind]); @@ -529,6 +565,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } + + printf("];\n"); ww /= sum2; wh /= sum2; hh /= sum2; @@ -546,9 +584,9 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt //cv::circle(rgbclone, cv::Point(w,h), 3, cv::Scalar(0,255,0)); -// cv::imshow( "rgbclone", rgbclone ); -// cv::waitKey(0); - + cv::imshow( "rgbclone", rgbclone ); + cv::waitKey(0); +/* pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); cloud2->width = width; @@ -599,9 +637,11 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt viewer->removeAllPointClouds(); viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud"); viewer->spin(); + */ } } } +} } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 78790c5d..94fd015a 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2156,7 +2156,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} - for(unsigned int w = 1; w < width;w++){ + for(unsigned int w = 1; w < width-1;w++){ for(unsigned int h = 1; h < height-1;h++){ int ind = h*width+w; float z = idepth*float(depthdata[ind]); @@ -2184,6 +2184,76 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb((z-z2)/(z*z+z2*z2));} } + +/* + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + int otherN = ind-dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + float z0 = idepth*float(depthdata[otherN]); + + if(z3 > 0){z2 = 2*z2-z3;} + + double diff1 = z-2*z2-z3; + double diff2 = z2-2*z-z0; + + if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb((z-z2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + //int other3 = ind-dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + if(z3 > 0){z2 = 2*z2-z3;} + + if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb((z-z2)/(z*z+z2*z2));} + } +*/ + /* + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + int otherN = ind-dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + float z0 = idepth*float(depthdata[otherN]); + + if(z3 > 0){z2 = 2*z2-z3;} + + double diff1 = z-2*z2-z3; + double diff2 = z2-2*z-z0; + + if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(std::min(diff1,diff2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + int otherN = ind-dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + float z0 = idepth*float(depthdata[otherN]); + + if(z3 > 0){z2 = 2*z2-z3;} + + double diff1 = z-2*z2-z3; + double diff2 = z2-2*z-z0; + + if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(std::min(diff1,diff2)/(z*z+z2*z2));} + } + */ } } @@ -2408,6 +2478,54 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int */ +std::vector doInference(std::vector & prior, std::vector< std::vector > & connectionId, std::vector< std::vector > & connectionStrength){ + + int nr_data = prior.size(); + + int nr_edges = 0; + for(unsigned int j = 0; j < nr_data;j++){ + nr_edges += connectionId[j].size(); + } + + printf("nr data: %i nr edges: %i\n",nr_data,nr_edges); + + gc::Graph * g = new gc::Graph(nr_data,nr_edges); + + + for(unsigned int i = 0; i < nr_data;i++){ + g -> add_node(); + double p_fg = prior[i]; + if(p_fg < 0){ + g -> add_tweights( i, 0, 0 ); + continue; + } + double p_bg = 1-p_fg; + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + g -> add_tweights( i, weightFG, weightBG ); + } + + for(unsigned int i = 0; i < nr_data;i++){ + for(unsigned int j = 0; j < connectionId[j].size();j++){ + double weight = connectionStrength[i][j]; + g -> add_edge( i, connectionId[i][j], weight, weight ); + } + } + + int flow = g -> maxflow(); + printf("flow: %i\n",flow); + + std::vector labels; + labels.resize(nr_data); + + for(unsigned int i = 0; i < nr_data;i++){ + labels[i] = g->what_segment(i); + } + delete g; + + return labels; +} + vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg){ std::vector newmasks; @@ -2419,6 +2537,27 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector::Ptr cloud1 (new pcl::PointCloud); pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); + int tot_nr_pixels = 0; + + for(unsigned int i = 0; i < frames.size(); i++){ + RGBDFrame * frame = frames[i]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + unsigned int nr_pixels = frame->camera->width * frame->camera->height; + tot_nr_pixels += nr_pixels; + } + + //Graph... + std::vector prior; + std::vector< std::vector > connectionId; + std::vector< std::vector > connectionStrength; + + for(unsigned int i = 0; i < frames.size(); i++){ RGBDFrame * frame = frames[i]; unsigned short * depthdata = (unsigned short *)(frame->depth.data); @@ -2455,16 +2594,77 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector > probs = getImageProbs(frames[i],5); + std::vector frame_prior; + std::vector< std::vector > frame_connectionId; + std::vector< std::vector > frame_connectionStrength; + + frame_prior.resize(nr_pixels); + frame_connectionId.resize(nr_pixels); + frame_connectionStrength.resize(nr_pixels); + + printf("starting partition\n"); double start_part = getTime(); int nr_data = width*height; - gc::Graph *g = new gc::Graph( nr_data, width*(height-1) + (width-1)*height); + int nr_edges = width*(height-1) + (width-1)*height; + gc::Graph *g = new gc::Graph( nr_data, nr_edges); + + printf("nr data: %i nr edges: %i\n",nr_data, nr_edges); + +// gc::Graph * g2 = new gc::Graph(nr_data,nr_edges); + +/* + int nr_data = prior.size(); + + int nr_edges = 0; + for(unsigned int j = 0; j < nr_data;j++){ + nr_edges += connectionId[j].size(); + } + + printf("nr data: %i nr edges: %i\n",nr_data,nr_edges); + + gc::Graph * g = new gc::Graph(nr_data,nr_edges); + for(unsigned int i = 0; i < nr_data;i++){ + g -> add_node(); + double p_fg = prior[i]; + if(p_fg < 0){ + g -> add_tweights( i, 0, 0 ); + continue; + } + double p_bg = 1-p_fg; + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + g -> add_tweights( i, weightFG, weightBG ); + } + + for(unsigned int i = 0; i < nr_data;i++){ + for(unsigned int j = 0; j < connectionId[j].size();j++){ + double weight = connectionStrength[i][j]; + g -> add_edge( i, connectionId[i][j], weight, weight ); + } + } + + int flow = g -> maxflow(); + printf("flow: %i\n",flow); + + std::vector labels; + labels.resize(nr_data); + + for(unsigned int i = 0; i < nr_data;i++){ + labels[i] = g->what_segment(i); + } + delete g; +*/ + + gc::Graph * g2 = new gc::Graph(nr_data,nr_edges); + for(unsigned int j = 0; j < nr_data;j++){ g -> add_node(); if(depthdata[j] == 0){ g -> add_tweights(j,0,0); + frame_prior[j] = -1; continue; } @@ -2475,20 +2675,34 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector add_tweights( j, weightFG, weightBG ); } +for(unsigned int j = 0; j < nr_data;j++){ + g2 -> add_node(); + double p_fg = frame_prior[j]; + if(p_fg < 0){ + g -> add_tweights( j, 0, 0 ); + continue; + } + double p_bg = 1-p_fg; + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + g2 -> add_tweights( j, weightFG, weightBG ); +} + + double maxprob_same = 0.999999999; - for(unsigned int w = 0; w < width-1;w++){ + for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; if(w > 0){ - - double ax = 0.5; double bx = 0.5; for(int p = 0; p < probs.size(); p+=2){ @@ -2502,7 +2716,12 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector add_edge( ind, other, weight, weight ); + //g2 -> add_edge( ind, other, weight, weight ); } if(h > 0){ @@ -2520,13 +2739,29 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector add_edge( ind, other, weight, weight ); + + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); + + g -> add_edge( ind, other, weight, weight ); + //g2 -> add_edge( ind, other, weight, weight ); } } } - int flow = g -> maxflow(); - printf("flow: %i\n",flow); + for(unsigned int ind = 0; ind < nr_data;ind++){ + for(unsigned int j = 0; j < frame_connectionId[ind].size();j++){ + int other = frame_connectionId[ind][j]; + double weight = frame_connectionStrength[ind][j]; + g2 -> add_edge(ind, other, weight, weight ); + } + } + + int flow = g -> maxflow(); + printf("flow: %i\n",flow); + + int flow2 = g2 -> maxflow(); + printf("flow2: %i\n",flow2); double end_part = getTime(); @@ -2651,7 +2886,16 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); +// int same = 0; +// int diff = 0; +// for(unsigned int i = 0; i < labels.size();i++){ +// if(labels[i] == g->what_segment(i)){same++;} +// else{diff++;} +// //internaldata[i] = (g->what_segment(i) == gc::Graph::SOURCE)); +// } +// printf("same: %i diff: %i\n",same,diff); cv::Mat internalmask; internalmask.create(height,width,CV_8UC1); From a7738291d306c050989ed9aa3fdb21f9e0cc0090 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 4 Aug 2016 23:02:49 +0200 Subject: [PATCH 048/255] multi-frame robustness --- .../quasimodo_models/src/core/RGBDFrame.cpp | 12 +++ .../src/modelupdater/ModelUpdater.cpp | 102 ++++++------------ 2 files changed, 44 insertions(+), 70 deletions(-) diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index d8b39a6b..68b06afd 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -393,6 +393,18 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt //show(true); } + + if(true){ + + for(int it = 0; it < 30; it++){ + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int ind = h*width+w; + } + } + } + } + if(false){ show(false); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 94fd015a..0f985e0f 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2506,7 +2506,7 @@ std::vector doInference(std::vector & prior, std::vector< std::vect } for(unsigned int i = 0; i < nr_data;i++){ - for(unsigned int j = 0; j < connectionId[j].size();j++){ + for(unsigned int j = 0; j < connectionId[i].size();j++){ double weight = connectionStrength[i][j]; g -> add_edge( i, connectionId[i][j], weight, weight ); } @@ -2538,17 +2538,11 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector::Ptr cloud2 (new pcl::PointCloud); int tot_nr_pixels = 0; + std::vector offsets; for(unsigned int i = 0; i < frames.size(); i++){ - RGBDFrame * frame = frames[i]; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - unsigned int nr_pixels = frame->camera->width * frame->camera->height; + offsets.push_back(tot_nr_pixels); + unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; tot_nr_pixels += nr_pixels; } @@ -2557,8 +2551,14 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector > connectionId; std::vector< std::vector > connectionStrength; + prior.resize(tot_nr_pixels); + connectionId.resize(tot_nr_pixels); + connectionStrength.resize(tot_nr_pixels); + + for(unsigned int i = 0; i < frames.size(); i++){ + int offset = offsets[i]; RGBDFrame * frame = frames[i]; unsigned short * depthdata = (unsigned short *)(frame->depth.data); unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); @@ -2611,52 +2611,6 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector * g2 = new gc::Graph(nr_data,nr_edges); - -/* - int nr_data = prior.size(); - - int nr_edges = 0; - for(unsigned int j = 0; j < nr_data;j++){ - nr_edges += connectionId[j].size(); - } - - printf("nr data: %i nr edges: %i\n",nr_data,nr_edges); - - gc::Graph * g = new gc::Graph(nr_data,nr_edges); - - - for(unsigned int i = 0; i < nr_data;i++){ - g -> add_node(); - double p_fg = prior[i]; - if(p_fg < 0){ - g -> add_tweights( i, 0, 0 ); - continue; - } - double p_bg = 1-p_fg; - double weightFG = -log(p_fg); - double weightBG = -log(p_bg); - g -> add_tweights( i, weightFG, weightBG ); - } - - for(unsigned int i = 0; i < nr_data;i++){ - for(unsigned int j = 0; j < connectionId[j].size();j++){ - double weight = connectionStrength[i][j]; - g -> add_edge( i, connectionId[i][j], weight, weight ); - } - } - - int flow = g -> maxflow(); - printf("flow: %i\n",flow); - - std::vector labels; - labels.resize(nr_data); - - for(unsigned int i = 0; i < nr_data;i++){ - labels[i] = g->what_segment(i); - } - delete g; -*/ gc::Graph * g2 = new gc::Graph(nr_data,nr_edges); @@ -2664,7 +2618,8 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector add_node(); if(depthdata[j] == 0){ g -> add_tweights(j,0,0); - frame_prior[j] = -1; + frame_prior[j] = -1; + prior[offset+j] = -1; continue; } @@ -2675,7 +2630,8 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector add_edge( ind, other, weight, weight ); //g2 -> add_edge( ind, other, weight, weight ); @@ -2743,6 +2702,9 @@ for(unsigned int j = 0; j < nr_data;j++){ frame_connectionId[ind].push_back(other); frame_connectionStrength[ind].push_back(weight); + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + g -> add_edge( ind, other, weight, weight ); //g2 -> add_edge( ind, other, weight, weight ); } @@ -2886,17 +2848,17 @@ for(unsigned int j = 0; j < nr_data;j++){ } -// std::vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); -// int same = 0; -// int diff = 0; -// for(unsigned int i = 0; i < labels.size();i++){ -// if(labels[i] == g->what_segment(i)){same++;} -// else{diff++;} -// //internaldata[i] = (g->what_segment(i) == gc::Graph::SOURCE)); -// } - -// printf("same: %i diff: %i\n",same,diff); + std::vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); + int same = 0; + int diff = 0; + for(unsigned int i = 0; i < labels.size();i++){ + if(labels[i] == g->what_segment(i)){same++;} + else{diff++;} + //internaldata[i] = (g->what_segment(i) == gc::Graph::SOURCE)); + } + printf("same: %i diff: %i\n",same,diff); +exit(0); cv::Mat internalmask; internalmask.create(height,width,CV_8UC1); unsigned char * internaldata = (unsigned char *)(internalmask.data); From 58c3bda7f82921553e931e8fbc0305f847a04a44 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 5 Aug 2016 15:43:19 +0200 Subject: [PATCH 049/255] fixed memmory leak in metaroom reader --- metaroom_xml_parser/src/simple_summary_parser.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/metaroom_xml_parser/src/simple_summary_parser.cpp b/metaroom_xml_parser/src/simple_summary_parser.cpp index 5e936f08..85362525 100644 --- a/metaroom_xml_parser/src/simple_summary_parser.cpp +++ b/metaroom_xml_parser/src/simple_summary_parser.cpp @@ -67,6 +67,9 @@ bool SimpleSummaryParser::createSummaryXML(std::string rootFolder, bool verbose) xmlWriter->writeEndDocument(); ROS_INFO_STREAM("Done looking for observations. Found "< Date: Fri, 5 Aug 2016 17:15:23 +0200 Subject: [PATCH 050/255] memmory profiling and adding of frame to frame inference --- .../quasimodo_brain/src/test_segment.cpp | 379 +------- .../quasimodo_models/include/model/Model.h | 2 + .../include/modelupdater/ModelUpdater.h | 2 +- .../quasimodo_models/src/core/Camera.cpp | 22 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 8 +- .../quasimodo_models/src/model/Model.cpp | 38 +- .../src/modelupdater/ModelUpdater.cpp | 890 ++++-------------- .../src/registration/MassRegistrationPPR2.cpp | 13 +- 8 files changed, 278 insertions(+), 1076 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index 66c42520..514f0fbb 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -328,58 +328,29 @@ reglib::Model * load2(std::string sweep_xml){ SimpleXMLParser parser; SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); - reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS - cam->fx = 532.158936; - cam->fy = 533.819214; - cam->cx = 310.514310; - cam->cy = 236.842039; +// projection +// 532.158936 0.000000 310.514310 0.000000 +// 0.000000 533.819214 236.842039 0.000000 +// 0.000000 0.000000 1.000000 0.000000 -/* - projection - 532.158936 0.000000 310.514310 0.000000 - 0.000000 533.819214 236.842039 0.000000 - 0.000000 0.000000 1.000000 0.000000 -*/ - -/* - [image] - - width - 640 - - height - 480 - - [narrow_stereo] - camera matrix - 534.192642 0.000000 311.485658 - 0.000000 533.870030 237.668175 - 0.000000 0.000000 1.000000 - - distortion - 0.030388 -0.100645 -0.000995 -0.000366 0.000000 - - rectification - 1.000000 0.000000 0.000000 - 0.000000 1.000000 0.000000 - 0.000000 0.000000 1.000000 - - projection - 532.158936 0.000000 310.514310 0.000000 - 0.000000 533.819214 236.842039 0.000000 - 0.000000 0.000000 1.000000 0.000000 -*/ - - cv::Mat fullmask; - fullmask.create(480,640,CV_8UC1); - unsigned char * maskdata = (unsigned char *)fullmask.data; - for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} reglib::Model * sweepmodel = 0; std::vector current_room_frames; for (size_t i=0; ifx = 532.158936; + cam->fy = 533.819214; + cam->cx = 310.514310; + cam->cy = 236.842039; + cout<<"Intermediate cloud size "<points.size()<points.size()); -/* - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( objectModel, reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; - - objectModel->print(); - mu->makeInitialSetup(); - objectModel->print(); - delete mu; - - objectModel->recomputeModelPoints(); -*/ -// reglib::Model * newmodelHolder = new reglib::Model(); -// newmodelHolder->submodels.push_back(sweepmodel); -// newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); -// newmodelHolder->recomputeModelPoints(); -/* - pcl::PointCloud::Ptr cn1 = objectModel->getPCLnormalcloud(1, false); - viewer->removeAllPointClouds(); - viewer->addPointCloud (cn1, pcl::visualization::PointCloudColorHandlerRGBField(cn1), "sample cloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); - viewer->spin(); -*/ - -// pcl::PointCloud::Ptr cn = sweepmodel->getPCLnormalcloud(1, false); -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cn, pcl::visualization::PointCloudColorHandlerRGBField(cn), "sample cloud"); -// viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); -// viewer->spin(); - return sweepmodel; } -void load(std::string sweep_xml){ - - int slash_pos = sweep_xml.find_last_of("/"); - std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; - printf("folder: %s\n",sweep_folder.c_str()); - - SimpleXMLParser parser; - SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); - - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); - vector objects = semantic_map_load_utilties::loadAllDynamicObjectsFromSingleSweep(sweep_folder+"room.xml"); - - int object_id; - for (unsigned int i=0; i< objects.size(); i++){ - if (objects[i].objectScanIndices.size() != 0){object_id = i;} - } - int index = objectFiles[object_id].toStdString().find_last_of("."); - string object_root_name = objectFiles[object_id].toStdString().substr(0,index); - - for (auto object : objects){ - if (!object.objectScanIndices.size()){continue;} - - nr_vAdditionalViews = 1 + rand()%5; - - std::vector viewrgbs; - std::vector viewdepths; - std::vector viewmasks; - std::vector viewtfs; - std::vector viewposes; - std::vector< image_geometry::PinholeCameraModel > viewcams; - char buf [1024]; - sprintf(buf,"%s/object_%i/poses.txt",sweep_folder.c_str(),object_id); - std::vector poses = getRegisteredViewPoses(std::string(buf), object.vAdditionalViews.size()); - cout<<"Loaded "<height,cloud->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)mask.data; - - cv::Mat rgb; - rgb.create(cloud->height,cloud->width,CV_8UC3); - unsigned char * rgbdata = (unsigned char *)rgb.data; - - cv::Mat depth; - depth.create(cloud->height,cloud->width,CV_16UC1); - unsigned short * depthdata = (unsigned short *)depth.data; - - unsigned int nr_data = cloud->height * cloud->width; - for(unsigned int j = 0; j < nr_data; j++){ - maskdata[j] = 0; - PointType p = cloud->points[j]; - rgbdata[3*j+0] = p.b; - rgbdata[3*j+1] = p.g; - rgbdata[3*j+2] = p.r; - depthdata[j] = short(5000.0 * p.z); - } - - cout<<"Processing AV "<>av_index; - av_mask->push_back(object.vAdditionalViews[i]->points[av_index]); - maskdata[av_index] = 255; - } - - viewrgbs.push_back(rgb); - viewdepths.push_back(depth); - viewmasks.push_back(mask); - viewtfs.push_back(object.vAdditionalViewsTransforms[i]); - viewposes.push_back(poses[i+1]); - viewcams.push_back(roomData.vIntermediateRoomCloudCamParams.front()); - - cv::namedWindow("rgbimage", cv::WINDOW_AUTOSIZE); - cv::imshow( "rgbimage", rgb); - cv::namedWindow("depthimage", cv::WINDOW_AUTOSIZE); - cv::imshow( "depthimage", depth); - cv::namedWindow("mask", cv::WINDOW_AUTOSIZE); - cv::imshow( "mask", mask); - cv::waitKey(30); - } - - if(viewrgbs.size() > 0){ - - rgbs.push_back(viewrgbs); - depths.push_back(viewdepths); - masks.push_back(viewmasks); - tfs.push_back(viewtfs); - initposes.push_back(viewposes); - cams.push_back(viewcams); -/* - tf::StampedTransform tf = roomData.vIntermediateRoomCloudTransforms.front(); - geometry_msgs::TransformStamped tfstmsg; - tf::transformStampedTFToMsg (tf, tfstmsg); - geometry_msgs::Transform tfmsg = tfstmsg.transform; - geometry_msgs::Pose pose; - pose.orientation = tfmsg.rotation; - pose.position.x = tfmsg.translation.x; - pose.position.y = tfmsg.translation.y; - pose.position.z = tfmsg.translation.z; - Eigen::Affine3d localthing; - tf::poseMsgToEigen(pose, localthing); - - reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS - cam->fx = viewcams.front().fx(); - cam->fy = viewcams.front().fy(); - cam->cx = viewcams.front().cx(); - cam->cy = viewcams.front().cy(); - - - cv::Mat fullmask; - fullmask.create(480,640,CV_8UC1); - unsigned char * maskdata = (unsigned char *)fullmask.data; - for(int j = 0; j < 480*640; j++){maskdata[j] = 0;} - - - reglib::Model * objectModel = 0;//new reglib::Model(frames[frame_id],mask); - - std::vector current_frames; - for (unsigned int i=0;i < nr_vAdditionalViews && iframes.push_back(frame); - objectModel->relativeposes.push_back(current_frames.front()->pose.inverse() * frame->pose); - objectModel->modelmasks.push_back(new reglib::ModelMask(viewmasks[i]));//fullmask)); - } - } - - reglib::Model * sweepmodel = 0; - std::vector current_room_frames; - for (size_t i=0; ipoints.size()<frames.push_back(frame); - sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose); - sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); - } - } - sweepmodel->recomputeModelPoints(); - -// objectModel->submodels.push_back(sweepmodel); -// objectModel->submodels_relativeposes.push_back(objectModel->frames.front()->pose.inverse() * sweepmodel->frames.front()->pose); - - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( objectModel, reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; - - objectModel->print(); - mu->makeInitialSetup(); - objectModel->print(); - delete mu; - - objectModel->recomputeModelPoints(); - - reglib::Model * newmodelHolder = new reglib::Model(); - newmodelHolder->submodels.push_back(objectModel); - newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); - newmodelHolder->recomputeModelPoints(); - - pcl::PointCloud::Ptr cn1 = objectModel->getPCLnormalcloud(1, false); - viewer->removeAllPointClouds(); - viewer->addPointCloud (cn1, pcl::visualization::PointCloudColorHandlerRGBField(cn1), "sample cloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); - viewer->spin(); - - pcl::PointCloud::Ptr cn = sweepmodel->getPCLnormalcloud(1, false); - viewer->removeAllPointClouds(); - viewer->addPointCloud (cn, pcl::visualization::PointCloudColorHandlerRGBField(cn), "sample cloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); - viewer->spin(); -*/ -// rgbs.push_back(viewrgbs); -// depths.push_back(viewdepths); -// masks.push_back(viewmasks); -// tfs.push_back(viewtfs); -// initposes.push_back(viewposes); -// cams.push_back(viewcams); - } +void runExitBehaviour(){ + for(int j = 0; j < models.size(); j++){ + models[j]->fullDelete(); } + printf("done\n"); + viewer.reset(); } int main(int argc, char** argv){ - ros::init(argc, argv, "test_segment"); - ros::NodeHandle n; -// ros::Subscriber sub = n.subscribe("modelserver", 1000, chatterCallback); - - viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0.5, 0, 0.5); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); - - ros::ServiceClient segmentation_client = n.serviceClient("segment_model"); - for(int ar = 1; ar < argc; ar++){ string overall_folder = std::string(argv[ar]); vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); @@ -702,28 +405,50 @@ int main(int argc, char** argv){ } } - //Not needed if metaroom well calibrated - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; + + +ros::init(argc, argv, "test_segment"); +ros::NodeHandle n; +// ros::Subscriber sub = n.subscribe("modelserver", 1000, chatterCallback); + +viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); +viewer->setBackgroundColor (0.5, 0, 0.5); +viewer->addCoordinateSystem (1.0); +viewer->initCameraParameters (); + +ros::ServiceClient segmentation_client = n.serviceClient("segment_model"); + + + //Not needed if metaroom well calibrated reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); + + + bgmassreg->timeout = 1200; bgmassreg->viewer = viewer; bgmassreg->use_surface = true; bgmassreg->use_depthedge = false; bgmassreg->visualizationLvl = 0; - bgmassreg->maskstep = 5; - bgmassreg->nomaskstep = 5; + bgmassreg->maskstep = 10; + bgmassreg->nomaskstep = 10; bgmassreg->nomask = true; bgmassreg->stopval = 0.0005; bgmassreg->setData(models.front()->frames,models.front()->modelmasks); - reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); + + delete bgmassreg; + runExitBehaviour(); + return 1; + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; for(int j = 0; j < models.size(); j++){ models[j]->relativeposes = bgmfr.poses; models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); diff --git a/quasimodo/quasimodo_models/include/model/Model.h b/quasimodo/quasimodo_models/include/model/Model.h index 4353745a..3c229360 100644 --- a/quasimodo/quasimodo_models/include/model/Model.h +++ b/quasimodo/quasimodo_models/include/model/Model.h @@ -95,6 +95,8 @@ class superpoint{ Model(); Model(RGBDFrame * frame_, cv::Mat mask, Eigen::Matrix4d pose = Eigen::Matrix4d::Identity()); ~Model(); + + void fullDelete(); void merge(Model * model, Eigen::Matrix4d p); diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index ccd885d8..c2c3ca36 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -93,7 +93,7 @@ namespace reglib virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); - virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask, double debugg = false); + virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength,double debugg = false); virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks); virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg = false); diff --git a/quasimodo/quasimodo_models/src/core/Camera.cpp b/quasimodo/quasimodo_models/src/core/Camera.cpp index 3c271050..42ccac0e 100644 --- a/quasimodo/quasimodo_models/src/core/Camera.cpp +++ b/quasimodo/quasimodo_models/src/core/Camera.cpp @@ -7,6 +7,8 @@ namespace reglib unsigned int camera_id_count = 0; Camera::Camera(){ + printf("new camera: %ld\n",this); + id = camera_id_count++; width = 640; @@ -17,20 +19,22 @@ Camera::Camera(){ cy = float(height-1)/2; idepth_scale = 0.001/5.0; - bias = 500; + pixel_weights = 0; + pixel_sums = 0; - pixel_weights = new double[width*height]; - pixel_sums = new double[width*height]; - for(unsigned int i = 0; i < width*height; i++){ - pixel_weights[i] = bias; - pixel_sums[i] = bias; - } +// bias = 500; +// pixel_weights = new double[width*height]; +// pixel_sums = new double[width*height]; +// for(unsigned int i = 0; i < width*height; i++){ +// pixel_weights[i] = bias; +// pixel_sums[i] = bias; +// } } Camera::~Camera(){ - delete[] pixel_weights; - delete[] pixel_sums; +// delete[] pixel_weights; +// delete[] pixel_sums; } void Camera::save(std::string path){ diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 68b06afd..d5480087 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -657,7 +657,13 @@ if(false){ } -RGBDFrame::~RGBDFrame(){} +RGBDFrame::~RGBDFrame(){ + rgb.release(); + normals.release(); + depth.release(); + depthedges.release(); + if(labels != 0){delete labels; labels = 0;} +} void RGBDFrame::show(bool stop){ cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 9388d110..bec979df 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -131,9 +131,9 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr float pny = m10*dst_nx + m11*dst_ny + m12*dst_nz; float pnz = m20*dst_nx + m21*dst_ny + m22*dst_nz; - float pb = rgbdata[3*ind+0]; - float pg = rgbdata[3*ind+1]; - float pr = rgbdata[3*ind+2]; + float pb = (float)(rgbdata[3*ind+0]); + float pg = (float)(rgbdata[3*ind+1]); + float pr = (float)(rgbdata[3*ind+2]); Vector3f pxyz (px ,py ,pz ); Vector3f pnxyz (pnx,pny,pnz); @@ -589,6 +589,38 @@ CloudData * Model::getCD(unsigned int target_points){ Model::~Model(){} +void Model::fullDelete(){ + points.clear(); + all_keypoints.clear(); + all_descriptors.clear(); + relativeposes.clear(); + for(size_t i = 0; i < frames.size(); i++){ + printf("delete camera: %ld\n",frames[i]->camera); + delete frames[i]->camera; + delete frames[i]; + } + frames.clear(); + + for(size_t i = 0; i < modelmasks.size(); i++){delete modelmasks[i];} + modelmasks.clear(); + + rep_relativeposes.clear(); + rep_frames.clear(); + rep_modelmasks.clear(); + + total_scores = 0; + scores.clear(); + + for(size_t i = 0; i < submodels.size(); i++){ + submodels[i]->fullDelete(); + } + submodels.clear(); + + submodels_relativeposes.clear(); + submodels_scores.clear(); + delete this; +} + pcl::PointCloud::Ptr Model::getPCLnormalcloud(int step, bool color){ pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); for(unsigned int i = 0; i < points.size(); i+=step){ diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 0f985e0f..62399557 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1464,10 +1464,7 @@ void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weight } } -double interpolate( double val, double y0, double x0, double y1, double x1 ) { - return (val-x0)*(y1-y0)/(x1-x0) + y0; -} - +double interpolate( double val, double y0, double x0, double y1, double x1 ) {return (val-x0)*(y1-y0)/(x1-x0) + y0;} double base( double val ) { if ( val <= -0.75 ) return 0; else if ( val <= -0.25 ) return interpolate( val, 0.0, -0.75, 1.0, -0.25 ); @@ -1475,22 +1472,13 @@ double base( double val ) { else if ( val <= 0.75 ) return interpolate( val, 1.0, 0.25, 0.0, 0.75 ); else return 0.0; } - -double red( double gray ) { - return base( gray - 0.5 ); -} -double green( double gray ) { - return base( gray ); -} -double blue( double gray ) { - return base( gray + 0.5 ); -} +double red( double gray ) { return base( gray - 0.5 ); } +double green( double gray ) { return base( gray );} +double blue( double gray ) { return base( gray + 0.5 );} cv::Mat getImageFromArray(unsigned int width, unsigned int height, double * arr){ double maxval = 0; - for(unsigned int i = 0; i < width*height; i++){ - maxval = std::max(maxval,arr[i]); - } + for(unsigned int i = 0; i < width*height; i++){maxval = std::max(maxval,arr[i]);} maxval = 1; cv::Mat m; @@ -1573,15 +1561,9 @@ printf("iter %i\n",iter); }else{ double fails = occlusion_penalty*(total[i][j]-overlaps[i][j]); double others = overlaps[i][j]; - wo[i][j] = others/(fails+others);//overlaps[i][j]/total[i][j]; - //wo[i][j] = wo[i][j]*0.9 + weights_old.back()[i][j]*0.9; + wo[i][j] = others/(fails+others); } } -//if(iter % 5 == 0){getImageFromArray(cf[i]->camera->width, cf[i]->camera->height, wo[i]);} -// printf("%i / %i \n",i+1,cf.size()); -// cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "depthedges", getImageFromArray(cf[i]->camera->width, cf[i]->camera->height, wo[i]) ); -// cv::waitKey(0); } weights_old.push_back(wo); } @@ -1607,7 +1589,7 @@ printf("iter %i\n",iter); } -void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, cv::Mat mask, double debugg){ +void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); float * src_normalsdata = (float *)(frame1->normals.data); @@ -1751,6 +1733,14 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE if(surface_angle > 0.8 && dst_maskdata[dst_ind] > 0){ + if(interframe_connectionId.size() > 0){ + double p_same = 0.9; + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + + interframe_connectionId[src_ind+offset1].push_back(dst_ind+offset2); + interframe_connectionStrength[src_ind+offset1].push_back(weight); + } overlaps[src_ind]++; if(debugg){ dst_cloud->points[dst_ind].r = 0; @@ -1783,181 +1773,6 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d } } -std::string type2str(int type) { - string r; - - uchar depth = type & CV_MAT_DEPTH_MASK; - uchar chans = 1 + (type >> CV_CN_SHIFT); - - switch ( depth ) { - case CV_8U: r = "8U"; break; - case CV_8S: r = "8S"; break; - case CV_16U: r = "16U"; break; - case CV_16S: r = "16S"; break; - case CV_32S: r = "32S"; break; - case CV_32F: r = "32F"; break; - case CV_64F: r = "64F"; break; - default: r = "User"; break; - } - - r += "C"; - r += (chans+'0'); - - return r; -} - -/** - * Code for thinning a binary image using Zhang-Suen algorithm. - * - * Author: Nash (nash [at] opencv-code [dot] com) - * Website: http://opencv-code.com - */ -#include - -/** - * Perform one thinning iteration. - * Normally you wouldn't call this function directly from your code. - * - * Parameters: - * im Binary image with range = [0,1] - * iter 0=even, 1=odd - */ -void thinningIteration(cv::Mat& img, int iter) -{ - CV_Assert(img.channels() == 1); - CV_Assert(img.depth() != sizeof(uchar)); - CV_Assert(img.rows > 3 && img.cols > 3); - - cv::Mat marker = cv::Mat::zeros(img.size(), CV_8UC1); - - int nRows = img.rows; - int nCols = img.cols; - - if (img.isContinuous()) { - nCols *= nRows; - nRows = 1; - } - - int x, y; - uchar *pAbove; - uchar *pCurr; - uchar *pBelow; - uchar *nw, *no, *ne; // north (pAbove) - uchar *we, *me, *ea; - uchar *sw, *so, *se; // south (pBelow) - - uchar *pDst; - - // initialize row pointers - pAbove = NULL; - pCurr = img.ptr(0); - pBelow = img.ptr(1); - - for (y = 1; y < img.rows-1; ++y) { - // shift the rows up by one - pAbove = pCurr; - pCurr = pBelow; - pBelow = img.ptr(y+1); - - pDst = marker.ptr(y); - - // initialize col pointers - no = &(pAbove[0]); - ne = &(pAbove[1]); - me = &(pCurr[0]); - ea = &(pCurr[1]); - so = &(pBelow[0]); - se = &(pBelow[1]); - - for (x = 1; x < img.cols-1; ++x) { - // shift col pointers left by one (scan left to right) - nw = no; - no = ne; - ne = &(pAbove[x+1]); - we = me; - me = ea; - ea = &(pCurr[x+1]); - sw = so; - so = se; - se = &(pBelow[x+1]); - - int A = (*no == 0 && *ne == 1) + (*ne == 0 && *ea == 1) + - (*ea == 0 && *se == 1) + (*se == 0 && *so == 1) + - (*so == 0 && *sw == 1) + (*sw == 0 && *we == 1) + - (*we == 0 && *nw == 1) + (*nw == 0 && *no == 1); - int B = *no + *ne + *ea + *se + *so + *sw + *we + *nw; - int m1 = iter == 0 ? (*no * *ea * *so) : (*no * *ea * *we); - int m2 = iter == 0 ? (*ea * *so * *we) : (*no * *so * *we); - - if (A == 1 && (B >= 2 && B <= 6) && m1 == 0 && m2 == 0) - pDst[x] = 1; - } - } - - img &= ~marker; -} - -/** - * Function for thinning the given binary image - * - * Parameters: - * src The source image, binary with range = [0,255] - * dst The destination image - */ -void thinning(const cv::Mat& src, cv::Mat& dst) -{ - dst = src.clone(); - dst /= 255; // convert to binary image - - cv::Mat prev = cv::Mat::zeros(dst.size(), CV_8UC1); - cv::Mat diff; - - do { - thinningIteration(dst, 0); - thinningIteration(dst, 1); - cv::absdiff(dst, prev, diff); - dst.copyTo(prev); - } - while (cv::countNonZero(diff) > 0); - - dst *= 255; -} - -void compute_thin_edges(std::vector< std::vector > probs,reglib::RGBDFrame * frame, int blursize = 5, double threshold = 0.5){ - unsigned int width = frame->rgb.cols; - unsigned int height = frame->rgb.rows; - - std::vector totprob; totprob.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totprob[i] = 0.5;} - std::vector totnprob; totnprob.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totnprob[i] = 0.5;} - for(unsigned int p = 0; p < probs.size()-2; p++){ - for(unsigned int i = 0; i < width*height;i++){ - totprob[i] *= probs[p][i]; - totnprob[i] *= 1.0- probs[p][i]; - } - } - - cv::Mat joint_prob; - joint_prob.create(height,width,CV_8UC1); - unsigned char * jointdata_prob = (unsigned char *)(joint_prob.data); - for(unsigned int i = 0; i < width*height;i++){ - jointdata_prob[i] = 255.0*((totprob[i]/(totprob[i]+totnprob[i])) < threshold); - } - - cv::imshow( "rgb", frame->rgb ); - cv::imshow( "joint_prob", joint_prob ); - cv::waitKey(50); - - int dilation_size = 1; - cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ); - cv::dilate( joint_prob, joint_prob, element ); - cv::imshow( "dilate", joint_prob ); - cv::waitKey(50); - - thinning(joint_prob, joint_prob); - cv::imshow( "skeleton", joint_prob ); - cv::waitKey(0); -} - std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ cv::Mat src = frame->rgb.clone(); unsigned short * depthdata = (unsigned short *)(frame->depth.data); @@ -1982,21 +1797,10 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int for(unsigned int h = 1; h < height-1;h++){ int ind = h*width+w; dir = 1; - //Xvec.push_back(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); dir = width; - //Xvec.push_back(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); - Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); -/* - dir = 1+width; - //Xvec.push_back(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); - Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - - dir = 1-width; - //Xvec.push_back(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - */ } } @@ -2023,7 +1827,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int func->maxnoise = stdval; func->reset(); func->computeModel(X); - printf("noise: %5.5f\n",func->getNoise()); + //printf("noise: %5.5f\n",func->getNoise()); std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} @@ -2032,37 +1836,18 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int for(unsigned int w = 1; w < width;w++){ for(unsigned int h = 1; h < height-1;h++){ int ind = h*width+w; -// dir = 1; -// dx[ind] = func->getProb(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); - -// dir = width; -// dy[ind] = func->getProb(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); - -// dir = 1+width; -// dxy[ind] = func->getProb(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); - -// dir = 1-width; -// dyx[ind] = func->getProb(0.5*fabs(2*srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c] - srcdata[chans*(ind+dir)+c])); dir = 1; dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); dir = width; dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - - dir = 1+width; - dxy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - - dir = 1-width; - dyx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); } } delete func; probs.push_back(dx); probs.push_back(dy); -// probs.push_back(dxy); -// probs.push_back(dyx); } { @@ -2184,76 +1969,6 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb((z-z2)/(z*z+z2*z2));} } - -/* - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; - int otherN = ind-dir; - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - float z0 = idepth*float(depthdata[otherN]); - - if(z3 > 0){z2 = 2*z2-z3;} - - double diff1 = z-2*z2-z3; - double diff2 = z2-2*z-z0; - - if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb((z-z2)/(z*z+z2*z2));} - } - - if(h > 1){ - int dir = -width; - int other2 = ind+2*dir; - int other = ind+dir; - //int other3 = ind-dir; - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - if(z3 > 0){z2 = 2*z2-z3;} - - if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb((z-z2)/(z*z+z2*z2));} - } -*/ - /* - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; - int otherN = ind-dir; - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - float z0 = idepth*float(depthdata[otherN]); - - if(z3 > 0){z2 = 2*z2-z3;} - - double diff1 = z-2*z2-z3; - double diff2 = z2-2*z-z0; - - if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(std::min(diff1,diff2)/(z*z+z2*z2));} - } - - if(h > 1){ - int dir = -width; - int other2 = ind+2*dir; - int other = ind+dir; - int otherN = ind-dir; - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - float z0 = idepth*float(depthdata[otherN]); - - if(z3 > 0){z2 = 2*z2-z3;} - - double diff1 = z-2*z2-z3; - double diff2 = z2-2*z-z0; - - if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(std::min(diff1,diff2)/(z*z+z2*z2));} - } - */ } } @@ -2261,237 +1976,19 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int probs.push_back(dx); probs.push_back(dy); } - -// std::vector totprob; totprob.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totprob[i] = 0.5;} -// std::vector totnprob; totnprob.resize(width*height); for(unsigned int i = 0; i < width*height;i++){totnprob[i] = 0.5;} -// for(unsigned int p = 0; p < probs.size(); p++){ -// for(unsigned int i = 0; i < width*height;i++){ -// totprob[i] *= probs[p][i]; -// totnprob[i] *= 1.0- probs[p][i]; -// } -// } - -// cv::Mat joint_prob; -// joint_prob.create(height,width,CV_8UC3); -// unsigned char * jointdata_prob = (unsigned char *)(joint_prob.data); -// for(unsigned int i = 0; i < width*height;i++){ -// jointdata_prob[3*i+0] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); -// jointdata_prob[3*i+1] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); -// jointdata_prob[3*i+2] = 255.0*totprob[i]/(totprob[i]+totnprob[i]); -// } - -// cv::imshow( "rgb", src ); -// cv::imshow( "joint_prob", joint_prob ); -// cv::waitKey(0); - return probs; } -/* - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - bool debugg1 = false; - if(debugg1){ - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - cloud->points.resize(width*height); - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - pcl::PointXYZRGBNormal point; - point.x = x; - point.y = y; - point.z = z; - if (g->what_segment(ind) == gc::Graph::SOURCE){ - point.r = 0; - point.g = 255; - point.b = 0; - }else{ - point.r = 255; - point.g = 0; - point.b = 0; - } - cloud->points[ind] = point; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - viewer->spin(); - } - delete g; - - bool debugg2 = false; - if(debugg2){ - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - cloud->points.resize(width*height); - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - - pcl::PointXYZRGBNormal point; - point.x = x; - point.y = y; - point.z = z; - point.r = 0; - point.g = 255; - point.b = 255; - - if(overlaps[ind] >= 1){ - point.r = 0; - point.g = 255; - point.b = 0; - } - if(occlusions[ind] >= 1){ - point.r = 255; - point.g = 0; - point.b = 0; - } - cloud->points[ind] = point; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - viewer->spin(); - } - - bool debugg3 = false; - if(debugg3){ - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - cloud->points.resize(width*height); - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - double ax = 0.5; - double bx = 0.5; - for(int p = 0; p < probs.size()-2; p+=2){ - double pr = probs[p][ind]; - ax *= pr; - bx *= 1.0-pr; - } - double px = ax/(ax+bx); - - double ay = 0.5; - double by = 0.5; - for(int p = 1; p < probs.size()-1; p+=2){ - double pr = probs[p][ind]; - ay *= pr; - by *= 1.0-pr; - } - double py = ay/(ay+by); - - double p_same_color = std::max(0.25, std::min(px,maxprob_same)); - double p_same_depth = std::min(probs[probs.size()-2][ind], maxprob_same); - double not_p_same_color = 1-p_same_color; - double not_p_same_depth = 1-p_same_depth; - double not_p_same = std::min(not_p_same_color,not_p_same_depth); - -// double ptest = (ax*ay)/(ax*ay+bx*by); - - pcl::PointXYZRGBNormal point; - point.x = x; - point.y = y; - point.z = z; - point.r = 255*not_p_same_color;//(1.0-probs[0][ind]); - point.g = 255*not_p_same_color;//*(1.0-probs[1][ind]); - point.b = 255*not_p_same_color; - cloud->points[ind] = point; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - viewer->spin(); - - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - double ax = 0.5; - double bx = 0.5; - for(int p = 0; p < probs.size()-2; p+=2){ - double pr = probs[p][ind]; - ax *= pr; - bx *= 1.0-pr; - } - double px = ax/(ax+bx); - - double ay = 0.5; - double by = 0.5; - for(int p = 1; p < probs.size()-1; p+=2){ - double pr = probs[p][ind]; - ay *= pr; - by *= 1.0-pr; - } - double py = ay/(ay+by); - - double p_same_color = std::max(0.25, std::min(py,maxprob_same)); - double p_same_depth = std::min(probs[probs.size()-1][ind], maxprob_same); - double not_p_same_color = 1-p_same_color; - double not_p_same_depth = 1-p_same_depth; - double not_p_same = std::min(not_p_same_color,not_p_same_depth); - - - pcl::PointXYZRGBNormal point; - point.x = x; - point.y = y; - point.z = z; - point.r = 255*not_p_same_color;//(1.0-probs[0][ind]); - point.g = 255*not_p_same_color;//*(1.0-probs[1][ind]); - point.b = 255*not_p_same_color; - cloud->points[ind] = point; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - viewer->spin(); - } - -*/ - std::vector doInference(std::vector & prior, std::vector< std::vector > & connectionId, std::vector< std::vector > & connectionStrength){ + double start_inf = getTime(); - int nr_data = prior.size(); - - int nr_edges = 0; + unsigned int nr_data = prior.size(); + unsigned int nr_edges = 0; for(unsigned int j = 0; j < nr_data;j++){ nr_edges += connectionId[j].size(); } - printf("nr data: %i nr edges: %i\n",nr_data,nr_edges); - gc::Graph * g = new gc::Graph(nr_data,nr_edges); - - for(unsigned int i = 0; i < nr_data;i++){ g -> add_node(); double p_fg = prior[i]; @@ -2512,17 +2009,16 @@ std::vector doInference(std::vector & prior, std::vector< std::vect } } - int flow = g -> maxflow(); - printf("flow: %i\n",flow); + g -> maxflow(); std::vector labels; labels.resize(nr_data); - - for(unsigned int i = 0; i < nr_data;i++){ - labels[i] = g->what_segment(i); - } + for(unsigned int i = 0; i < nr_data;i++){labels[i] = g->what_segment(i);} delete g; + double end_inf = getTime(); + printf("nr data: %i nr edges: %i inference time: %10.10fs\n",nr_data,nr_edges,end_inf-start_inf); + return labels; } @@ -2550,10 +2046,14 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector prior; std::vector< std::vector > connectionId; std::vector< std::vector > connectionStrength; + std::vector< std::vector > interframe_connectionId; + std::vector< std::vector > interframe_connectionStrength; - prior.resize(tot_nr_pixels); - connectionId.resize(tot_nr_pixels); - connectionStrength.resize(tot_nr_pixels); + prior.resize(tot_nr_pixels); + connectionId.resize(tot_nr_pixels); + connectionStrength.resize(tot_nr_pixels); + interframe_connectionId.resize(tot_nr_pixels); + interframe_connectionStrength.resize(tot_nr_pixels); @@ -2576,11 +2076,13 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector > interframe_connectionId_tmp; + std::vector< std::vector > interframe_connectionStrength_tmp; for(unsigned int j = 0; j < bgcp.size(); j++){ if(frame == bgcf[j]){continue;} Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],false); + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); } for(unsigned int j = 0; j < nr_pixels; j++){ @@ -2590,7 +2092,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector > probs = getImageProbs(frames[i],5); @@ -2605,19 +2107,19 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector *g = new gc::Graph( nr_data, nr_edges); + unsigned int nr_data = width*height; +// int nr_edges = width*(height-1) + (width-1)*height; +// gc::Graph *g = new gc::Graph( nr_data, nr_edges); - printf("nr data: %i nr edges: %i\n",nr_data, nr_edges); +// printf("nr data: %i nr edges: %i\n",nr_data, nr_edges); - gc::Graph * g2 = new gc::Graph(nr_data,nr_edges); +// gc::Graph * g2 = new gc::Graph(nr_data,nr_edges); for(unsigned int j = 0; j < nr_data;j++){ - g -> add_node(); +// g -> add_node(); if(depthdata[j] == 0){ - g -> add_tweights(j,0,0); +// g -> add_tweights(j,0,0); frame_prior[j] = -1; prior[offset+j] = -1; continue; @@ -2633,24 +2135,24 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector add_tweights( j, weightFG, weightBG ); +// double p_bg = 1-p_fg; +// double weightFG = -log(p_fg); +// double weightBG = -log(p_bg); +// g -> add_tweights( j, weightFG, weightBG ); } -for(unsigned int j = 0; j < nr_data;j++){ - g2 -> add_node(); - double p_fg = frame_prior[j]; - if(p_fg < 0){ - g -> add_tweights( j, 0, 0 ); - continue; - } - double p_bg = 1-p_fg; - double weightFG = -log(p_fg); - double weightBG = -log(p_bg); - g2 -> add_tweights( j, weightFG, weightBG ); -} +//for(unsigned int j = 0; j < nr_data;j++){ +// g2 -> add_node(); +// double p_fg = frame_prior[j]; +// if(p_fg < 0){ +// g -> add_tweights( j, 0, 0 ); +// continue; +// } +// double p_bg = 1-p_fg; +// double weightFG = -log(p_fg); +// double weightBG = -log(p_bg); +// g2 -> add_tweights( j, weightFG, weightBG ); +//} double maxprob_same = 0.999999999; @@ -2661,7 +2163,7 @@ for(unsigned int j = 0; j < nr_data;j++){ if(w > 0){ double ax = 0.5; double bx = 0.5; - for(int p = 0; p < probs.size(); p+=2){ + for(unsigned int p = 0; p < probs.size(); p+=2){ double pr = probs[p][ind]; ax *= pr; bx *= 1.0-pr; @@ -2679,7 +2181,7 @@ for(unsigned int j = 0; j < nr_data;j++){ connectionId[ind+offset].push_back(other+offset); connectionStrength[ind+offset].push_back(weight); - g -> add_edge( ind, other, weight, weight ); +// g -> add_edge( ind, other, weight, weight ); //g2 -> add_edge( ind, other, weight, weight ); } @@ -2687,7 +2189,7 @@ for(unsigned int j = 0; j < nr_data;j++){ double ay = 0.5; double by = 0.5; - for(int p = 1; p < probs.size(); p+=2){ + for(unsigned int p = 1; p < probs.size(); p+=2){ double pr = probs[p][ind]; ay *= pr; by *= 1.0-pr; @@ -2705,30 +2207,32 @@ for(unsigned int j = 0; j < nr_data;j++){ connectionId[ind+offset].push_back(other+offset); connectionStrength[ind+offset].push_back(weight); - g -> add_edge( ind, other, weight, weight ); - //g2 -> add_edge( ind, other, weight, weight ); +// g -> add_edge( ind, other, weight, weight ); +// g2 -> add_edge( ind, other, weight, weight ); } } } - for(unsigned int ind = 0; ind < nr_data;ind++){ - for(unsigned int j = 0; j < frame_connectionId[ind].size();j++){ - int other = frame_connectionId[ind][j]; - double weight = frame_connectionStrength[ind][j]; - g2 -> add_edge(ind, other, weight, weight ); - } - } +// for(unsigned int ind = 0; ind < nr_data;ind++){ +// for(unsigned int j = 0; j < frame_connectionId[ind].size();j++){ +// int other = frame_connectionId[ind][j]; +// double weight = frame_connectionStrength[ind][j]; +// g2 -> add_edge(ind, other, weight, weight ); +// } +// } - int flow = g -> maxflow(); - printf("flow: %i\n",flow); +// int flow = g -> maxflow(); +// printf("flow: %i\n",flow); - int flow2 = g2 -> maxflow(); - printf("flow2: %i\n",flow2); +// int flow2 = g2 -> maxflow(); +// printf("flow2: %i\n",flow2); double end_part = getTime(); printf("part time: %10.10fs\n",end_part-start_part); + std::vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); + Eigen::Matrix4d p = poses[i]; float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); @@ -2791,7 +2295,7 @@ for(unsigned int j = 0; j < nr_data;j++){ point.x = tx; point.y = ty; point.z = tz; - if (g->what_segment(ind) == gc::Graph::SOURCE){ + if (labels[ind] == gc::Graph::SOURCE){ point.r = 0; point.g = 255; point.b = 0; @@ -2847,23 +2351,22 @@ for(unsigned int j = 0; j < nr_data;j++){ } } + for(unsigned int j = 0; j < nr_pixels; j++){ + occlusions[j] = 0; + } - std::vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); - int same = 0; - int diff = 0; - for(unsigned int i = 0; i < labels.size();i++){ - if(labels[i] == g->what_segment(i)){same++;} - else{diff++;} - //internaldata[i] = (g->what_segment(i) == gc::Graph::SOURCE)); - } + for(unsigned int j = 0; j < frames.size(); j++){ + if(frame == frames[j]){continue;} + Eigen::Matrix4d p = poses[i].inverse() * poses[j]; + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, frames[j],masks[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); + } - printf("same: %i diff: %i\n",same,diff); -exit(0); cv::Mat internalmask; internalmask.create(height,width,CV_8UC1); unsigned char * internaldata = (unsigned char *)(internalmask.data); for(unsigned int i = 0; i < width*height;i++){ - internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); + //internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); + internaldata[i] = 255.0*((depthdata[i] == 0) || (labels[i] == gc::Graph::SOURCE)); } newmasks.push_back(internalmask); @@ -2871,137 +2374,77 @@ exit(0); delete[] overlaps; delete[] occlusions; } -/* - if(debugg){ - for(double weight = 2000; weight < 10000; weight *= 2){ - printf("weight: %f\n",weight); - DenseCRF crf (cloud->points.size(),2); - - double poseScale = 0.1; - //double jointColorScale = 20.0; - //double jointPoseScale = 0.05; - MatrixXf featureJoint( 6, cloud->points.size() ); - MatrixXf featurePose( 3, cloud->points.size() ); - MatrixXf unary( 2, cloud->points.size() ); - for(unsigned int k=0; k < unaryprobs.size(); k++ ){ - double p_fg = unaryprobs[k]; - double p_bg = 1-p_fg; - - double weightFG = -log(p_fg); - double weightBG = -log(p_bg); - - unary(0,k) = weightFG/weight; - unary(1,k) = weightBG/weight; - - pcl::PointXYZRGBNormal point = cloud->points[k]; - featureJoint(0,k) = point.x/0.05; - featureJoint(1,k) = point.y/0.05; - featureJoint(2,k) = point.z/0.05; - featureJoint(3,k) = point.r/60.0; - featureJoint(4,k) = point.g/60.0; - featureJoint(5,k) = point.b/60.0; - - featurePose(0,k) = point.x/0.001; - featurePose(1,k) = point.y/0.001; - featurePose(2,k) = point.z/0.001; - } - crf.setUnaryEnergy( unary ); - crf.addPairwiseEnergy(featureJoint,new PottsCompatibility( 10 ),DIAG_KERNEL,NORMALIZE_SYMMETRIC); - crf.addPairwiseEnergy(featurePose,new PottsCompatibility( 10 ),DIAG_KERNEL,NORMALIZE_SYMMETRIC); - VectorXs map = crf.map(25); + std::vector global_labels = doInference(prior,connectionId,connectionStrength); + for(unsigned int i = 0; i < interframe_connectionId.size(); i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size(); j++){ + connectionId[i].push_back(interframe_connectionId[i][j]); + connectionStrength[i].push_back(interframe_connectionStrength[i][j]); + } + } + std::vector improvedglobal_labels = doInference(prior,connectionId,connectionStrength); - pcl::PointCloud::Ptr cloud3 (new pcl::PointCloud); - for(unsigned int k = 0; k < cloud->points.size(); k++ ){ - pcl::PointXYZRGBNormal point = cloud->points[k]; - if( map(k) == 0 ){ - point.r = 255; - point.g = 0; - point.b = 0; - }else{ - point.r = 0; - point.g = 255; - point.b = 0; - } - cloud3->points.push_back(point); - } + for(unsigned int f = 0; f < frames.size(); f++){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud3, pcl::visualization::PointCloudColorHandlerRGBField(cloud3), "scloud"); - viewer->spin(); - } - } -*/ - /* - - // add a color dependent term (feature = xyrgb) - // x_stddev = 60 - // y_stddev = 60 - // r_stddev = g_stddev = b_stddev = 20 - // weight = 10 - - - crf.addPairwiseGaussian( 3, 3, new PottsCompatibility( 3 ) ); - void DenseCRF2D::addPairwiseGaussian ( float sx, float sy, LabelCompatibility * function, KernelType kernel_type, NormalizationType normalization_type ) { - MatrixXf feature( 2, N_ ); - for( int j=0; jdepth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - crf.addPairwiseBilateral( 80, 80, 13, 13, 13, im, new PottsCompatibility( 10 ) ); - void DenseCRF2D::addPairwiseBilateral ( float sx, float sy, float sr, float sg, float sb, const unsigned char* im, LabelCompatibility * function, KernelType kernel_type, NormalizationType normalization_type ) { - MatrixXf feature( 5, N_ ); - for( int j=0; jcamera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + + unsigned int nr_pixels = frame->camera->width * frame->camera->height; + + cv::Mat originalmask; + originalmask.create(height,width,CV_8UC1); + unsigned char * originaldata = (unsigned char *)(originalmask.data); + + + cv::Mat improvedmask; + improvedmask.create(height,width,CV_8UC1); + unsigned char * improveddata = (unsigned char *)(improvedmask.data); + + + cv::Mat diffmask; + diffmask.create(height,width,CV_8UC3); + unsigned char * diffdata = (unsigned char *)(diffmask.data); + + for(unsigned int i = 0; i < nr_pixels;i++){ + //internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); + originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph::SOURCE)); + improveddata[i] = 255.0*((depthdata[i] == 0) || (improvedglobal_labels[i+offset] == gc::Graph::SOURCE)); + if(improveddata[i] < originaldata[i]){ + diffdata[3*i+0] = 0; + diffdata[3*i+1] = 255; + diffdata[3*i+2] = 0; + }else if(improveddata[i] > originaldata[i]){ + diffdata[3*i+0] = 0; + diffdata[3*i+1] = 0; + diffdata[3*i+2] = 255; + }else{ + diffdata[3*i+0] = 0; + diffdata[3*i+1] = 0; + diffdata[3*i+2] = 0; } - addPairwiseEnergy( feature, function, kernel_type, normalization_type ); - } - */ -/* - /////////// Put your own unary classifier here! /////////// - MatrixXf unary = computeUnary( getLabeling( anno, W*H, M ), M ); - /////////////////////////////////////////////////////////// - - // Setup the CRF model - DenseCRF2D crf(W, H, M); - // Specify the unary potential as an array of size W*H*(#classes) - // packing order: x0y0l0 x0y0l1 x0y0l2 .. x1y0l0 x1y0l1 ... - crf.setUnaryEnergy( unary ); - // add a color independent term (feature = pixel location 0..W-1, 0..H-1) - // x_stddev = 3 - // y_stddev = 3 - // weight = 3 - crf.addPairwiseGaussian( 3, 3, new PottsCompatibility( 3 ) ); - // add a color dependent term (feature = xyrgb) - // x_stddev = 60 - // y_stddev = 60 - // r_stddev = g_stddev = b_stddev = 20 - // weight = 10 - crf.addPairwiseBilateral( 80, 80, 13, 13, 13, im, new PottsCompatibility( 10 ) ); - - // Do map inference -// MatrixXf Q = crf.startInference(), t1, t2; -// printf("kl = %f\n", crf.klDivergence(Q) ); -// for( int it=0; it<5; it++ ) { -// crf.stepInference( Q, t1, t2 ); -// printf("kl = %f\n", crf.klDivergence(Q) ); -// } -// VectorXs map = crf.currentMap(Q); -*/ + } + frame->show(false); + cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); + cv::imshow( "Original", originalmask ); + cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); + cv::imshow( "Improved", improvedmask ); + cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); + cv::imshow( "Difference", diffmask ); + cv::waitKey(0); + //newmasks.push_back(internalmask); + } if(debugg){ @@ -3016,26 +2459,6 @@ exit(0); viewer->removeAllPointClouds(); viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "scloud"); viewer->spin(); - - -// pcl::PointCloud::Ptr cloud3 (new pcl::PointCloud); -// for(unsigned int k=0; k < unaryprobs.size(); k++ ){ -// pcl::PointXYZRGBNormal point = cloud->points[k]; -// if(map(k) == 0){ -// point.r = 255; -// point.g = 0; -// point.b = 0; -// }else{ -// point.r = 0; -// point.g = 255; -// point.b = 0; -// } -// cloud3->points.push_back(point); -// } - -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud3, pcl::visualization::PointCloudColorHandlerRGBField(cloud3), "scloud"); -// viewer->spin(); } return newmasks; @@ -3060,16 +2483,25 @@ std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eig } + std::vector< std::vector > interframe_connectionId_tmp; + std::vector< std::vector > interframe_connectionStrength_tmp; + +// for(unsigned int j = 0; j < bgcp.size(); j++){ +// if(frame == bgcf[j]){continue;} +// Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; +// getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); +// } + for(unsigned int j = 0; j < cf.size(); j++){ if(i == j){continue;} Eigen::Matrix4d p = cp[i].inverse() * cp[j]; - getDynamicWeights(false,p.inverse(), cf[i], overlaps, occlusions, cf[j],oldmasks[j]); + getDynamicWeights(false,p.inverse(), cf[i], overlaps, occlusions, cf[j],oldmasks[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp); } std::vector< std::vector > probs = getImageProbs(cf[i],5); printf("starting partition\n"); double start_part = getTime(); - int nr_data = width*height; + unsigned int nr_data = width*height; gc::Graph *g = new gc::Graph( nr_data, width*(height-1) + (width-1)*height); double maxprob = 0.7; @@ -3100,7 +2532,7 @@ std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eig double ax = 0.5; double bx = 0.5; - for(int p = 0; p < probs.size(); p+=2){ + for(unsigned int p = 0; p < probs.size(); p+=2){ double pr = probs[p][ind]; ax *= pr; bx *= 1.0-pr; @@ -3109,7 +2541,7 @@ std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eig double ay = 0.5; double by = 0.5; - for(int p = 1; p < probs.size(); p+=2){ + for(unsigned int p = 1; p < probs.size(); p+=2){ double pr = probs[p][ind]; ay *= pr; by *= 1.0-pr; diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 0c2e2111..586688a2 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -717,16 +717,16 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect } matchscores[i][j] = ratiosum; - printf("%i %i -> %5.5f\n",i,j,ratiosum); + //printf("%i %i -> %5.5f\n",i,j,ratiosum); overlapping += ratiosum > 0; work_done++; } } - printf("ignores_overlap: %i/%i -> %5.5f\n",ignores_overlap,nr_frames*nr_frames - nr_frames, double(ignores_overlap)/double(nr_frames*nr_frames - nr_frames)); - printf("ignores_motion: %i/%i -> %5.5f\n",ignores_motion,nr_frames*nr_frames - nr_frames, double(ignores_motion)/double(nr_frames*nr_frames - nr_frames)); - printf("total work done: %i/%i -> %5.5f\n",work_done,nr_frames*nr_frames - nr_frames, double(work_done)/double(nr_frames*nr_frames - nr_frames)); + //printf("ignores_overlap: %i/%i -> %5.5f\n",ignores_overlap,nr_frames*nr_frames - nr_frames, double(ignores_overlap)/double(nr_frames*nr_frames - nr_frames)); + //printf("ignores_motion: %i/%i -> %5.5f\n",ignores_motion,nr_frames*nr_frames - nr_frames, double(ignores_motion)/double(nr_frames*nr_frames - nr_frames)); + //printf("total work done: %i/%i -> %5.5f\n",work_done,nr_frames*nr_frames - nr_frames, double(work_done)/double(nr_frames*nr_frames - nr_frames)); } if(rmt==1){ @@ -3127,8 +3127,9 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectorcomputeModel(depthedge_all_residuals); } - printf("surface reg: %5.5f noise: %5.5f\n",func->regularization,func->noiseval); - printf("edge: reg: %5.5f noise: %5.5f\n",depthedge_func->regularization,depthedge_func->noiseval); + //printf("surface reg: %5.5f noise: %5.5f\n",func->regularization,func->noiseval); + //printf("edge: reg: %5.5f noise: %5.5f\n",depthedge_func->regularization,depthedge_func->noiseval); + // kpfunc->computeModel(all_KPresiduals); //stopval = func->getNoise()*0.1; // printf("reg: %f noise:%f stopval: %f\n",func->regularization,func->noiseval,stopval); From e42de0831ceaab7863d00500715d8a4f82da6059 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Sat, 6 Aug 2016 20:34:21 +0200 Subject: [PATCH 051/255] fixed like 100 memleaks in simple_xml_parse.h --- .../metaroom_xml_parser/simple_xml_parser.h | 69 ++++++++++--------- 1 file changed, 37 insertions(+), 32 deletions(-) diff --git a/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h b/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h index b977b060..7ff86883 100644 --- a/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h +++ b/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h @@ -253,7 +253,8 @@ friend class SimpleDynamicObjectParser; } if (xmlReader->name() == "RoomRunNumber") { - aRoom.roomRunNumber = xmlReader->readElementText().toInt(); + QString tmp = xmlReader->readElementText(); + aRoom.roomRunNumber = atoi(tmp.toStdString().c_str());//xmlReader->readElementText().toInt(); } if (xmlReader->name() == "RoomLogStartTime") @@ -325,7 +326,10 @@ friend class SimpleDynamicObjectParser; QStringList transformSlist = transformS.split(' '); for (size_t i=0; i<16;i++) { - transform(i)=transformSlist[i].toDouble(); + //QString tmp = xmlReader->readElementText(); + //aRoom.roomRunNumber = atoi(tmp.toStdString().c_str());//xmlReader->readElementText().toInt(); + transform(i)=atof(transformSlist[i].toStdString().c_str()); + //transform(i)=transformSlist[i].toDouble(); } aRoom.roomTransform = transform; } @@ -382,12 +386,13 @@ friend class SimpleDynamicObjectParser; { if (xmlReader.name() == "sec") { - int sec = xmlReader.readElementText().toInt(); + + int sec = atoi(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toInt(); tfmsg.header.stamp.sec = sec; } if (xmlReader.name() == "nsec") { - int nsec = xmlReader.readElementText().toInt(); + int nsec = atoi(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toInt(); tfmsg.header.stamp.nsec = nsec; } if (xmlReader.name() == "FrameId") @@ -410,12 +415,12 @@ friend class SimpleDynamicObjectParser; } if (xmlReader.name() == "w") { - double w = xmlReader.readElementText().toDouble(); + double w = atof(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toDouble(); tfmsg.transform.rotation.w = w; } if (xmlReader.name() == "x") { - double x = xmlReader.readElementText().toDouble(); + double x = atof(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toDouble(); if (intermediateParentNode == "Rotation") { tfmsg.transform.rotation.x = x; @@ -427,7 +432,7 @@ friend class SimpleDynamicObjectParser; } if (xmlReader.name() == "y") { - double y = xmlReader.readElementText().toDouble(); + double y = atof(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toDouble(); if (intermediateParentNode == "Rotation") { tfmsg.transform.rotation.y = y; @@ -439,7 +444,7 @@ friend class SimpleDynamicObjectParser; } if (xmlReader.name() == "z") { - double z = xmlReader.readElementText().toDouble(); + double z = atof(xmlReader.readElementText().toStdString().c_str());//xmlReader.readElementText().toDouble(); if (intermediateParentNode == "Rotation") { tfmsg.transform.rotation.z = z; @@ -470,7 +475,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("height")) { QString val = paramAttributes.value("height").toString(); - camInfo.height = val.toInt(); + camInfo.height = atoi(val.toStdString().c_str());//val.toInt(); } else { ROS_ERROR("RoomIntermediateCameraParameters xml node does not have the height attribute. Cannot construct camera parameters object."); @@ -480,7 +485,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("width")) { QString val = paramAttributes.value("width").toString(); - camInfo.width = val.toInt(); + camInfo.width = atoi(val.toStdString().c_str());//val.toInt(); } else { ROS_ERROR("RoomIntermediateCameraParameters xml node does not have the width attribute. Cannot construct camera parameters object."); @@ -518,7 +523,7 @@ friend class SimpleDynamicObjectParser; } else { for (size_t j=0; j< val_list.size();j++) { - camInfo.K[j] = val_list[j].toDouble(); + camInfo.K[j] = atof(val_list[j].toStdString().c_str());//val_list[j].toDouble(); } } } else { @@ -537,7 +542,7 @@ friend class SimpleDynamicObjectParser; } else { for (size_t j=0; j< val_list.size();j++) { - camInfo.D.push_back(val_list[j].toDouble()); + camInfo.D.push_back(atof(val_list[j].toStdString().c_str()));//val_list[j].toDouble()); } } } else { @@ -556,7 +561,7 @@ friend class SimpleDynamicObjectParser; } else { for (size_t j=0; j< val_list.size();j++) { - camInfo.R[j] = val_list[j].toDouble(); + camInfo.R[j] = atof(val_list[j].toStdString().c_str());//val_list[j].toDouble(); } } } else { @@ -575,7 +580,7 @@ friend class SimpleDynamicObjectParser; } else { for (size_t j=0; j< val_list.size();j++) { - camInfo.P[j] = val_list[j].toDouble(); + camInfo.P[j] = atof(val_list[j].toStdString().c_str());//val_list[j].toDouble(); } } } else { @@ -636,13 +641,13 @@ friend class SimpleDynamicObjectParser; // read in number of depth and rgb images int numRGB, numDepth; if (attributes.hasAttribute("RGB_Images")) - { - numRGB = attributes.value("RGB_Images").toString().toInt(); + {//atoi(val.toStdString().c_str());// + numRGB = atoi(attributes.value("RGB_Images").toString().toStdString().c_str());//.toInt(); } if (attributes.hasAttribute("Depth_Images")) { - numDepth = attributes.value("Depth_Images").toString().toInt(); + numDepth = atoi(attributes.value("Depth_Images").toString().toStdString().c_str());//.toInt(); } // read in the images @@ -723,7 +728,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("height")) { QString val = paramAttributes.value("height").toString(); - camInfo.height = val.toInt(); + camInfo.height = atoi(val.toStdString().c_str());//val.toInt(); } else { ROS_ERROR("%s xml node does not have the height attribute. Cannot construct camera parameters object.",xmlReader->name().toString().toStdString().c_str()); @@ -733,7 +738,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("width")) { QString val = paramAttributes.value("width").toString(); - camInfo.width = val.toInt(); + camInfo.width = atoi(val.toStdString().c_str());//val.toInt(); } else { ROS_ERROR("%s xml node does not have the width attribute. Cannot construct camera parameters object.",xmlReader->name().toString().toStdString().c_str()); @@ -771,7 +776,7 @@ friend class SimpleDynamicObjectParser; } else { for (size_t j=0; j< val_list.size();j++) { - camInfo.K[j] = val_list[j].toDouble(); + camInfo.K[j] = atof(val_list[j].toStdString().c_str());//toDouble(); } } } else { @@ -790,7 +795,7 @@ friend class SimpleDynamicObjectParser; } else { for (size_t j=0; j< val_list.size();j++) { - camInfo.D.push_back(val_list[j].toDouble()); + camInfo.D.push_back(atof(val_list[j].toStdString().c_str()));//.toDouble()); } } } else { @@ -809,7 +814,7 @@ friend class SimpleDynamicObjectParser; } else { for (size_t j=0; j< val_list.size();j++) { - camInfo.R[j] = val_list[j].toDouble(); + camInfo.R[j] = atof(val_list[j].toStdString().c_str());//val_list[j].toDouble(); } } } else { @@ -828,7 +833,7 @@ friend class SimpleDynamicObjectParser; } else { for (size_t j=0; j< val_list.size();j++) { - camInfo.P[j] = val_list[j].toDouble(); + camInfo.P[j] = atof(val_list[j].toStdString().c_str());//val_list[j].toDouble(); } } } else { @@ -869,7 +874,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("Stamp_sec")) { QString val = paramAttributes.value("Stamp_sec").toString(); - tfmsg.header.stamp.sec = val.toInt(); + tfmsg.header.stamp.sec = atoi(val.toStdString().c_str());//val.toInt(); } else { ROS_ERROR("%s xml node does not have the Stamp_sec attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str()); @@ -878,7 +883,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("Stamp_nsec")) { QString val = paramAttributes.value("Stamp_nsec").toString(); - tfmsg.header.stamp.nsec = val.toInt(); + tfmsg.header.stamp.nsec = atoi(val.toStdString().c_str());//val.toInt(); } else { ROS_ERROR("%s xml node does not have the Stamp_nsec attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str()); @@ -905,7 +910,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("Trans_x")) { QString val = paramAttributes.value("Trans_x").toString(); - tfmsg.transform.translation.x = val.toDouble(); + tfmsg.transform.translation.x = atof(val.toStdString().c_str());//val.toDouble(); } else { ROS_ERROR("%s xml node does not have the Trans_x attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str()); @@ -914,7 +919,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("Trans_y")) { QString val = paramAttributes.value("Trans_y").toString(); - tfmsg.transform.translation.y = val.toDouble(); + tfmsg.transform.translation.y = atof(val.toStdString().c_str());//val.toDouble(); } else { ROS_ERROR("%s xml node does not have the Trans_y attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str()); @@ -923,7 +928,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("Trans_z")) { QString val = paramAttributes.value("Trans_z").toString(); - tfmsg.transform.translation.z = val.toDouble(); + tfmsg.transform.translation.z = atof(val.toStdString().c_str());//val.toDouble(); } else { ROS_ERROR("%s xml node does not have the Trans_z attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str()); @@ -932,7 +937,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("Rot_w")) { QString val = paramAttributes.value("Rot_w").toString(); - tfmsg.transform.rotation.w = val.toDouble(); + tfmsg.transform.rotation.w = atof(val.toStdString().c_str());//val.toDouble(); } else { ROS_ERROR("%s xml node does not have the Rot_w attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str()); @@ -941,7 +946,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("Rot_x")) { QString val = paramAttributes.value("Rot_x").toString(); - tfmsg.transform.rotation.x = val.toDouble(); + tfmsg.transform.rotation.x = atof(val.toStdString().c_str());//val.toDouble(); } else { ROS_ERROR("%s xml node does not have the Rot_x attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str()); @@ -950,7 +955,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("Rot_y")) { QString val = paramAttributes.value("Rot_y").toString(); - tfmsg.transform.rotation.y = val.toDouble(); + tfmsg.transform.rotation.y = atof(val.toStdString().c_str());//val.toDouble(); } else { ROS_ERROR("%s xml node does not have the Rot_y attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str()); @@ -959,7 +964,7 @@ friend class SimpleDynamicObjectParser; if (paramAttributes.hasAttribute("Rot_z")) { QString val = paramAttributes.value("Rot_z").toString(); - tfmsg.transform.rotation.z = val.toDouble(); + tfmsg.transform.rotation.z = atof(val.toStdString().c_str());//val.toDouble(); } else { ROS_ERROR("%s xml node does not have the Rot_z attribute. Cannot construct tf::StampedTransform.", xmlReader->name().toString().toStdString().c_str()); From 504f0d3d3bf441986892be9e2c5e08d407b58c74 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 8 Aug 2016 23:26:52 +0200 Subject: [PATCH 052/255] removing leaks --- .../quasimodo_brain/src/test_segment.cpp | 334 ++++-------------- .../quasimodo_models/include/core/RGBDFrame.h | 2 + .../quasimodo_models/src/core/RGBDFrame.cpp | 10 +- .../quasimodo_models/src/model/Model.cpp | 74 ++-- .../src/registration/MassRegistrationPPR2.cpp | 190 +++++++++- 5 files changed, 294 insertions(+), 316 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index 514f0fbb..622145c3 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -107,217 +107,6 @@ std::vector getRegisteredViewPoses(const std::string& poses_fil return toRet; } -void chatterCallback(const std_msgs::String::ConstPtr& msg) -{ - ROS_INFO("I heard: [%s]", msg->data.c_str()); - - for(unsigned int i = 1; i < rgbs.size(); i++){ - std::vector fid; - std::vector fadded; - for(unsigned int j = 0; j < rgbs[i].size(); j++){ - printf("%i %i\n",i,j); - - //cv::Mat maskimage = masks[i][j]; - cv::Mat rgbimage = rgbs[i][j]; - cv::Mat depthimage = depths[i][j]; - tf::StampedTransform tf = tfs[i][j]; - Eigen::Matrix4f m = initposes[i][j]; - geometry_msgs::TransformStamped tfstmsg; - tf::transformStampedTFToMsg (tf, tfstmsg); - - geometry_msgs::Transform tfmsg = tfstmsg.transform; - - printf("start adding frame %i\n",i); - - Eigen::Quaternionf q = Eigen::Quaternionf(Eigen::Affine3f(m).rotation()); - - geometry_msgs::Pose pose; -// pose.orientation = tfmsg.rotation; -// pose.position.x = tfmsg.translation.x; -// pose.position.y = tfmsg.translation.y; -// pose.position.z = tfmsg.translation.z; - pose.orientation.x = q.x(); - pose.orientation.y = q.y(); - pose.orientation.z = q.z(); - pose.orientation.w = q.w(); - pose.position.x = m(0,3); - pose.position.y = m(1,3); - pose.position.z = m(2,3); - - - cv_bridge::CvImage rgbBridgeImage; - rgbBridgeImage.image = rgbimage; - rgbBridgeImage.encoding = "bgr8"; - - cv_bridge::CvImage depthBridgeImage; - depthBridgeImage.image = depthimage; - depthBridgeImage.encoding = "mono16"; -/* - quasimodo_msgs::index_frame ifsrv; - ifsrv.request.frame.capture_time = ros::Time(); - ifsrv.request.frame.pose = pose; - ifsrv.request.frame.frame_id = -1; - ifsrv.request.frame.rgb = *(rgbBridgeImage.toImageMsg()); - ifsrv.request.frame.depth = *(depthBridgeImage.toImageMsg()); - - - //314.458000 242.038000 536.458000 537.422000 - ifsrv.request.frame.camera.K[0] = 536.458000;//cams[i][j].fx(); - ifsrv.request.frame.camera.K[4] = 537.422000;//cams[i][j].fy(); - ifsrv.request.frame.camera.K[2] = 314.458000;//cams[i][j].cx(); - ifsrv.request.frame.camera.K[5] = 242.038000;//cams[i][j].cy(); - - printf("camera: %f %f %f %f\n",cams[i][j].cx(),cams[i][j].cy(),cams[i][j].fx(),cams[i][j].fy()); - - if (index_frame_client.call(ifsrv)){//Add frame to model server - int frame_id = ifsrv.response.frame_id; - fadded.push_back(j); - fid.push_back(frame_id); - ROS_INFO("frame_id%i", frame_id ); - }else{ROS_ERROR("Failed to call service index_frame");} -*/ - printf("stop adding frame %i\n",i); - } - - for(unsigned int j = 0; j < fadded.size(); j++){ - printf("start adding mask %i\n",i); - cv::Mat mask = masks[i][fadded[j]]; - - cv_bridge::CvImage maskBridgeImage; - maskBridgeImage.image = mask; - maskBridgeImage.encoding = "mono8"; -/* - quasimodo_msgs::model_from_frame mff; - mff.request.mask = *(maskBridgeImage.toImageMsg()); - mff.request.isnewmodel = (j == (fadded.size()-1)); - mff.request.frame_id = fid[j]; - - if (model_from_frame_client.call(mff)){//Build model from frame - int model_id = mff.response.model_id; - if(model_id > 0){ - ROS_INFO("model_id%i", model_id ); - } - }else{ROS_ERROR("Failed to call service index_frame");} -*/ - printf("stop adding mask %i\n",i); - - } - } -} - -pcl::PointCloud::Ptr transformCloud(pcl::PointCloud::Ptr cloud, Eigen::Matrix4d p){ - pcl::PointCloud::Ptr ret (new pcl::PointCloud); - ret->points.resize(cloud->points.size()); - - const double & m00 = p(0,0); const double & m01 = p(0,1); const double & m02 = p(0,2); const double & m03 = p(0,3); - const double & m10 = p(1,0); const double & m11 = p(1,1); const double & m12 = p(1,2); const double & m13 = p(1,3); - const double & m20 = p(2,0); const double & m21 = p(2,1); const double & m22 = p(2,2); const double & m23 = p(2,3); - - for(unsigned int i = 0; i < cloud->points.size(); i++){ - pcl::PointXYZRGBNormal & p1 = cloud->points[i]; - pcl::PointXYZRGBNormal & p2 = ret->points[i]; - - p2.r = p1.r; - p2.g = p1.g; - p2.b = p1.b; - - const double & src_x = p1.x; - const double & src_y = p1.y; - const double & src_z = p1.z; - - const double & src_nx = p1.normal_x; - const double & src_ny = p1.normal_y; - const double & src_nz = p1.normal_z; - - p2.x = m00*src_x + m01*src_y + m02*src_z + m03; - p2.y = m10*src_x + m11*src_y + m12*src_z + m13; - p2.z = m20*src_x + m21*src_y + m22*src_z + m23; - - p2.normal_x = m00*src_nx + m01*src_ny + m02*src_nz; - p2.normal_y = m10*src_nx + m11*src_ny + m12*src_nz; - p2.normal_z = m20*src_nx + m21*src_ny + m22*src_nz; - - p2.r = p1.r; - p2.g = p1.g; - p2.b = p1.b; - } - - return ret; -} - -pcl::PointCloud::Ptr getNC(pcl::PointCloud::Ptr cloud, int nr_data = 5000000){ - int MaxDepthChangeFactor = 20; - int NormalSmoothingSize = 7; - int depth_dependent_smoothing = 1; - - pcl::IntegralImageNormalEstimation ne; - ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); - ne.setNormalSmoothingSize(NormalSmoothingSize); - ne.setDepthDependentSmoothing (depth_dependent_smoothing); - ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX); - - pcl::PointCloud::Ptr normals (new pcl::PointCloud); - ne.setInputCloud(cloud); - ne.compute(*normals); - - int rc = rand()%256; - int gc = rand()%256; - int bc = rand()%256; - - pcl::PointCloud::Ptr ret (new pcl::PointCloud); - pcl::concatenateFields(*cloud,*normals,*ret); - - boost::shared_ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0.5, 0, 0.5); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "sample cloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); - viewer->spin(); - - viewer->removeAllPointClouds(); - viewer->addPointCloud (ret, pcl::visualization::PointCloudColorHandlerRGBField(ret), "sample cloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud"); - viewer->addPointCloudNormals (ret, ret, 15, 0.10, "normals"); - viewer->spin(); - - -// ret->points.resize(cloud->points.size()); -// for(int i = 0; i < cloud->points.size(); i++){ -// pcl::PointXYZRGBNormal & p1 = ret->points[i]; -// pcl::PointXYZRGB & p = cloud->points[i]; -// pcl::Normal & pn = normals->points[i]; - -// p1.x = p.x; -// p1.y = p.y; -// p1.z = p.z; -//// p1.r = rc; -//// p1.g = gc; -//// p1.b = bc; -// p1.r = p.r; -// p1.g = p.g; -// p1.b = p.b; -// p1.normal_x = pn.normal_x; -// p1.normal_y = pn.normal_y; -// p1.normal_z = pn.normal_z; -// } -/* - -*/ - -// for(int i = 0; i < ret->points.size(); i++){ -// pcl::PointXYZRGBNormal & p1 = ret->points[i]; -// p1.r = rc; -// p1.g = gc; -// p1.b = bc; -// } - return ret; -} - -int nr_vAdditionalViews = 4; - std::vector models; reglib::Model * load2(std::string sweep_xml){ @@ -333,11 +122,10 @@ reglib::Model * load2(std::string sweep_xml){ // 0.000000 533.819214 236.842039 0.000000 // 0.000000 0.000000 1.000000 0.000000 - - reglib::Model * sweepmodel = 0; + std::vector current_room_frames; - for (size_t i=0; icx = 310.514310; cam->cy = 236.842039; + cout<<"Intermediate cloud size "<points.size()<modelmasks.push_back(new reglib::ModelMask(fullmask)); } } - sweepmodel->recomputeModelPoints(); - models.push_back(sweepmodel); - printf("nr points: %i\n",sweepmodel->points.size()); + //sweepmodel->recomputeModelPoints(); + printf("nr points: %i\n",sweepmodel->points.size()); +// sweepmodel->fullDelete(); +// delete sweepmodel; + + + models.push_back(sweepmodel); return sweepmodel; } void runExitBehaviour(){ - for(int j = 0; j < models.size(); j++){ - models[j]->fullDelete(); - } +// for(int j = 0; j < models.size(); j++){ +// models[j]->fullDelete(); +// } printf("done\n"); viewer.reset(); } int main(int argc, char** argv){ + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + +// viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); +// viewer->setBackgroundColor (0.5, 0, 0.5); +// viewer->addCoordinateSystem (1.0); +// viewer->initCameraParameters (); + + for(int ar = 1; ar < argc; ar++){ + string overall_folder = std::string(argv[ar]); + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + printf("sweep_xmls\n"); + for (auto sweep_xml : sweep_xmls) { + printf("sweep_xml: %s\n",sweep_xml.c_str()); + load2(sweep_xml); + } + } + +//Not needed if metaroom well calibrated + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); + bgmassreg->timeout = 20; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = false; + bgmassreg->visualizationLvl = 0; + bgmassreg->maskstep = 10; + bgmassreg->nomaskstep = 10; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->setData(models.front()->frames,models.front()->modelmasks); + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); + delete bgmassreg; - for(int ar = 1; ar < argc; ar++){ - string overall_folder = std::string(argv[ar]); - vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); - printf("sweep_xmls\n"); - for (auto sweep_xml : sweep_xmls) { - printf("sweep_xml: %s\n",sweep_xml.c_str()); - load2(sweep_xml); - } - } +// reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); +// mu->occlusion_penalty = 15; +// mu->massreg_timeout = 60*4; +// mu->viewer = viewer; + for(size_t j = 0; j < models.size(); j++){ +// //models[j]->relativeposes = bgmfr.poses; +// models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + } -ros::init(argc, argv, "test_segment"); -ros::NodeHandle n; -// ros::Subscriber sub = n.subscribe("modelserver", 1000, chatterCallback); -viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); -viewer->setBackgroundColor (0.5, 0, 0.5); -viewer->addCoordinateSystem (1.0); -viewer->initCameraParameters (); -ros::ServiceClient segmentation_client = n.serviceClient("segment_model"); - //Not needed if metaroom well calibrated - reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); +ros::init(argc, argv, "test_segment"); +ros::NodeHandle n; +ros::ServiceClient segmentation_client = n.serviceClient("segment_model"); - bgmassreg->timeout = 1200; - bgmassreg->viewer = viewer; - bgmassreg->use_surface = true; - bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = 0; - bgmassreg->maskstep = 10; - bgmassreg->nomaskstep = 10; - bgmassreg->nomask = true; - bgmassreg->stopval = 0.0005; - bgmassreg->setData(models.front()->frames,models.front()->modelmasks); - delete bgmassreg; - runExitBehaviour(); - return 1; - reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; - for(int j = 0; j < models.size(); j++){ - models[j]->relativeposes = bgmfr.poses; - models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - } +// + // for(int i = 0; i < models.front()->relativeposes.size(); i++){ // models.front()->frames[i]->camera->print(); @@ -463,15 +261,25 @@ ros::ServiceClient segmentation_client = n.serviceClient 0){ sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]); } + if (segmentation_client.call(sm)){//Build model from frame //int model_id = mff.response.model_id; printf("segmented: %i\n",i); }else{ROS_ERROR("Failed to call service segment_model");} + */ } + delete reg; + //viewer.reset(); + for(size_t j = 0; j < models.size(); j++){ + models[j]->fullDelete(); + delete models[j]; + } + return 0; // mff.request.mask = *(maskBridgeImage.toImageMsg()); // mff.request.isnewmodel = (j == (fadded.size()-1)); diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 44526b6d..24ee427e 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -34,6 +34,8 @@ namespace reglib Eigen::Matrix4d pose; int sweepid; + float * rgbdata; + cv::Mat rgb; cv::Mat depth; cv::Mat normals; diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index d5480087..707c0709 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -60,8 +60,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt sweepid = -1; id = RGBDFrame_id_counter++; camera = camera_; - rgb = rgb_; - depth = depth_; + rgb = rgb_.clone(); + depth = depth_.clone(); capturetime = capturetime_; pose = pose_; @@ -92,8 +92,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt intersections[0].resize(1); intersections[0][0] = 0; - nr_labels = 1; - labels = new int[width*height]; + nr_labels = 1; + labels = new int[width*height]; for(int i = 0; i < width*height; i++){labels[i] = 0;} @@ -662,7 +662,7 @@ RGBDFrame::~RGBDFrame(){ normals.release(); depth.release(); depthedges.release(); - if(labels != 0){delete labels; labels = 0;} + if(labels != 0){delete[] labels; labels = 0;} } void RGBDFrame::show(bool stop){ diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index bec979df..c6f1b8a9 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -29,10 +29,11 @@ Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){ relativeposes.push_back(pose); frames.push_back(frame); modelmasks.push_back(new ModelMask(mask)); - recomputeModelPoints(); + //recomputeModelPoints(); } void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask){ + printf("addSuperPoints\n"); bool * maskvec = modelmask->maskvec; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); unsigned short * depthdata = (unsigned short *)(frame->depth.data); @@ -67,9 +68,12 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr bool * isfused = new bool[width*height]; for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} - +printf("%i\n",__LINE__); for(unsigned int ind = 0; ind < spvec.size();ind++){ + if(ind % 1000 == 0){ + //printf("ind: %i\n",ind); + } superpoint & sp = spvec[ind]; float src_x = sp.point(0); @@ -146,7 +150,7 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr } } } - +printf("%i\n",__LINE__); int nr_fused = 0; int nr_mask = 0; for(unsigned int w = 0; w < width; w++){ @@ -156,7 +160,7 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr nr_mask += maskvec[ind] > 0; } } - +printf("%i\n",__LINE__); for(unsigned int w = 0; w < width; w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; @@ -193,7 +197,7 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr } } } - +printf("%i\n",__LINE__); delete[] isfused; } @@ -361,6 +365,7 @@ std::vector::Ptr> ret; } void Model::addAllSuperPoints(std::vector & spvec, Eigen::Matrix4d pose){ + printf("addAllSuperPoints\n"); for(unsigned int i = 0; i < frames.size(); i++){ addSuperPoints(spvec, pose*relativeposes[i], frames[i], modelmasks[i]); } @@ -371,16 +376,8 @@ void Model::addAllSuperPoints(std::vector & spvec, Eigen::Matrix4d p } void Model::recomputeModelPoints(Eigen::Matrix4d pose){ -// for(unsigned int i = 0; i < frames.size(); i++){ -// bool res = testFrame(i); -// } - points.clear(); - addAllSuperPoints(points,pose); - -// vector spvec; -// addAllSuperPoints(spvec,pose); -// points = spvec; + addAllSuperPoints(points,pose); } void Model::addPointsToModel(RGBDFrame * frame, ModelMask * modelmask, Eigen::Matrix4d p){ @@ -590,35 +587,36 @@ CloudData * Model::getCD(unsigned int target_points){ Model::~Model(){} void Model::fullDelete(){ - points.clear(); - all_keypoints.clear(); - all_descriptors.clear(); - relativeposes.clear(); - for(size_t i = 0; i < frames.size(); i++){ - printf("delete camera: %ld\n",frames[i]->camera); - delete frames[i]->camera; - delete frames[i]; - } - frames.clear(); + points.clear(); + all_keypoints.clear(); + all_descriptors.clear(); + relativeposes.clear(); + for(size_t i = 0; i < frames.size(); i++){ + printf("delete camera: %ld\n",frames[i]->camera); + delete frames[i]->camera; + delete frames[i]; + } + frames.clear(); - for(size_t i = 0; i < modelmasks.size(); i++){delete modelmasks[i];} - modelmasks.clear(); + for(size_t i = 0; i < modelmasks.size(); i++){delete modelmasks[i];} + modelmasks.clear(); - rep_relativeposes.clear(); - rep_frames.clear(); - rep_modelmasks.clear(); + rep_relativeposes.clear(); + rep_frames.clear(); + rep_modelmasks.clear(); - total_scores = 0; - scores.clear(); + total_scores = 0; + scores.clear(); - for(size_t i = 0; i < submodels.size(); i++){ - submodels[i]->fullDelete(); - } - submodels.clear(); + for(size_t i = 0; i < submodels.size(); i++){ + submodels[i]->fullDelete(); + delete submodels[i]; + } + submodels.clear(); - submodels_relativeposes.clear(); - submodels_scores.clear(); - delete this; + submodels_relativeposes.clear(); + submodels_scores.clear(); +// delete this; } pcl::PointCloud::Ptr Model::getPCLnormalcloud(int step, bool color){ diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 586688a2..40b9f35d 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -82,14 +82,6 @@ MassRegistrationPPR2::MassRegistrationPPR2(double startreg, bool visualize){ } MassRegistrationPPR2::~MassRegistrationPPR2(){ - for(unsigned int i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} - for(unsigned int i = 0; i < arraynormals.size(); i++){delete[] arraynormals[i];} - for(unsigned int i = 0; i < arraycolors.size(); i++){delete[] arraycolors[i];} - for(unsigned int i = 0; i < arrayinformations.size(); i++){delete[] arrayinformations[i];} - - for(unsigned int i = 0; i < trees3d.size(); i++){delete trees3d[i];} - for(unsigned int i = 0; i < a3dv.size(); i++){delete a3dv[i];} - delete func; delete kpfunc; delete depthedge_func; @@ -104,7 +96,13 @@ MassRegistrationPPR2::~MassRegistrationPPR2(){ delete[] kp_Qn_arr; delete[] kp_Xp_arr; delete[] kp_Xn_arr; - delete[] kp_rangeW_arr; + delete[] kp_rangeW_arr; + + delete[] depthedge_Qp_arr; + delete[] depthedge_Xp_arr; + delete[] depthedge_rangeW_arr; + + clearData(); } void MassRegistrationPPR2::addModel(Model * model){ @@ -1165,7 +1163,179 @@ Eigen::MatrixXd MassRegistrationPPR2::getAllKpResiduals(std::vector Date: Tue, 9 Aug 2016 17:12:50 +0200 Subject: [PATCH 053/255] working on segmentation --- .../src/segmentationserver.cpp | 425 ++++-------------- .../quasimodo_brain/src/test_segment.cpp | 183 +------- .../quasimodo_models/src/model/Model.cpp | 19 +- .../src/modelupdater/ModelUpdater.cpp | 320 +++++++------ 4 files changed, 284 insertions(+), 663 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index cdca18c9..5217f5cf 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -45,22 +45,6 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs models.push_back(quasimodo_brain::getModelFromMSG(req.models[i])); } -// reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.0); -// bgmassreg->timeout = 1200; -// bgmassreg->viewer = viewer; -// bgmassreg->visualizationLvl = 1; -// bgmassreg->maskstep = 10; -// bgmassreg->nomaskstep = 10; -// bgmassreg->nomask = true; -// bgmassreg->stopval = 0.0005; -// bgmassreg->setData(models.front()->frames,models.front()->modelmasks); -// reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); - -// for(int i = 0; i < models.front()->relativeposes.size(); i++){ -// models.front()->frames[i]->camera->print(); -// std::cout << models.front()->relativeposes[i] << std::endl << std::endl; -// } - reglib::Model * bg = quasimodo_brain::getModelFromMSG(req.backgroundmodel); reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.25); @@ -72,14 +56,7 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs massregmod->nomask = true; massregmod->stopval = 0.0001; -/* - bg->relativeposes = bgmfr.poses; - bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); - for(int j = 0; j < models.size(); j++){ - models[j]->relativeposes = bgmfr.poses; - models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - } -*/ + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); mu->occlusion_penalty = 15; @@ -126,6 +103,9 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs } } + delete massregmod; + + vector bgcp; vector bgcf; vector bgmask; @@ -155,8 +135,8 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs masks.push_back(mask); } - std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions - std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,true);//Determine external occlusions + std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,true);//Determine self occlusions + std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,false);//Determine external occlusions std::vector dynamic_masks; for(unsigned int i = 0; i < model->frames.size(); i++){ reglib::RGBDFrame * frame = model->frames[i]; @@ -178,11 +158,11 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs dynamic_masks.push_back(mask); - cv::imshow( "rgb", frame->rgb ); - cv::imshow( "internal_masks", internal_masks[i] ); - cv::imshow( "externalmask", external_masks[i] ); - cv::imshow( "dynamic_mask", dynamic_masks[i] ); - cv::waitKey(0); +// cv::imshow( "rgb", frame->rgb ); +// cv::imshow( "internal_masks", internal_masks[i] ); +// cv::imshow( "externalmask", external_masks[i] ); +// cv::imshow( "dynamic_mask", dynamic_masks[i] ); +// cv::waitKey(0); } internal.push_back(internal_masks); @@ -190,222 +170,88 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs dynamic.push_back(dynamic_masks); } -// vector updated_bgcp; -// vector updated_bgcf; -// vector updated_bgmask; - - - -/* - - - printf("time to go!\n"); - //std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,cp,cf,masks,cp,cf,masks,true);//Determine self occlusions - - - std::vector external_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,bgcp,bgcf,bgmask,cp,cf,masks,true);//Determine self occlusions -*/ - -/* - -exit(0); -// printf("frames: %i\n",bg->frames.size()); -// for(int i = 0; i < bg->frames.size(); i++){ -// cv::imshow("modelmask", bg->modelmasks[i]->getMask()); -// bg->frames[i]->show(true); -// } -// printf("points: %i\n",bg->points.size()); - -// viewer->removeAllPointClouds(); -// pcl::PointCloud::Ptr cloud = bg->getPCLcloud(1, false); -// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); -// viewer->spin(); -// printf("cloudsize: %i\n",cloud->points.size()); - vector cp; - vector cf; - vector mm; - - vector cp_front; - vector cf_front; - vector mm_front; - - - if(bg->frames.size() > 0){ - cp_front.push_back(Eigen::Matrix4d::Identity()); - cf_front.push_back(bg->frames.front()); - mm_front.push_back(bg->modelmasks.front()); - - for(int j = 0; j < models.size(); j++){ - cp_front.push_back(bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); - cf_front.push_back(models[j]->frames.front()); - mm_front.push_back(models[j]->modelmasks.front()); - } - }else{ - for(int j = 0; j < models.size(); j++){ - cp_front.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); - cf_front.push_back(models[j]->frames.front()); - mm_front.push_back(models[j]->modelmasks.front()); - } - } + for(unsigned int i = 0; i < models.size(); i++){ + std::vector internal_masks = internal[i]; + std::vector external_masks = external[i]; + std::vector dynamic_masks = dynamic[i]; + reglib::Model * model = models[i]; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + for(unsigned int j = 0; j < model->frames.size(); j++){ + reglib::RGBDFrame * frame = model->frames[j]; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + reglib::Camera * camera = frame->camera; - printf("cp_front: %i\n",cp_front.size()); - - if(models.size() > 1 || (models.size() > 0 && bg->frames.size() > 0)){ - printf("%s::%i\n",__FILE__,__LINE__); - reglib::MassRegistrationPPR2 * massreg = new reglib::MassRegistrationPPR2(0.05); - massreg->timeout = 1200; - massreg->viewer = viewer; - massreg->visualizationLvl = 0; - - massreg->maskstep = 5;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massreg->nomaskstep = 5;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massreg->nomask = true; - massreg->stopval = 0.0005; - - massreg->setData(cf_front,mm_front); - - - reglib::MassFusionResults mfr_front = massreg->getTransforms(cp_front); - for(int j = 0; j < mfr_front.poses.size(); j++){ - std::cout << mfr_front.poses[j] * mfr_front.poses.front().inverse() << std::endl << std::endl << std::endl; - } - - if(bg->frames.size() > 0){ - printf("change:\n"); - for(int j = models.size()-1; j >= 0; j--){ - Eigen::Matrix4d change = mfr_front.poses[j+1] * cp_front[j].inverse(); - std::cout << change << std::endl << std::endl << std::endl; - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); - cf.push_back(models[j]->frames[k]); - mm.push_back(models[j]->modelmasks[k]); - } - } - }else{ - printf("change:\n"); - for(int j = models.size()-1; j >= 0; j--){ - Eigen::Matrix4d change = mfr_front.poses[j] * cp_front[j].inverse(); - std::cout << change << std::endl << std::endl << std::endl; - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); - cf.push_back(models[j]->frames[k]); - mm.push_back(models[j]->modelmasks[k]); - } - } - } - }else{ - printf("%s::%i\n",__FILE__,__LINE__); - for(int j = models.size()-1; j >= 0; j--){ - Eigen::Matrix4d change = Eigen::Matrix4d::Identity();//mfr_front.poses[j] * cp_front[j].inverse(); - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); - cf.push_back(models[j]->frames[k]); - mm.push_back(models[j]->modelmasks[k]); + cv::imshow( "rgb", frame->rgb ); + cv::imshow( "internal_masks", internal_masks[j] ); + cv::imshow( "externalmask", external_masks[j] ); + cv::imshow( "dynamic_mask", dynamic_masks[j] ); + cv::waitKey(100); + + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); + unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); + + Eigen::Matrix4d p = model->relativeposes[j]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + pcl::PointXYZRGBNormal point; + point.x = m00*x + m01*y + m02*z + m03; + point.y = m10*x + m11*y + m12*z + m13; + point.z = m20*x + m21*y + m22*z + m23; + + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; + + if(dynamicmaskdata[ind] != 0){ + point.b = 0; + point.g = 255; + point.r = 0; + }else if(internalmaskdata[ind] == 0){ + point.b = 0; + point.g = 0; + point.r = 255; + } + + cloud->points.push_back(point); + } + } } } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + while(cv::waitKey(50)!='q'){viewer->spinOnce();} } - printf("cp: %i\n",cp.size()); - - reglib::MassRegistrationPPR2 * massreg2 = new reglib::MassRegistrationPPR2(0.05); - massreg2->timeout = 1200; - massreg2->viewer = viewer; - massreg2->visualizationLvl = 1; - - massreg2->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massreg2->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massreg2->nomask = true; - massreg2->stopval = 0.0005; - - massreg2->setData(cf,mm); - - if(bg->frames.size() > 0){ - massreg2->addModel(bg); - cp.push_back(Eigen::Matrix4d::Identity()); - printf("---->added background model\n"); - } - - reglib::MassFusionResults mfr2 = massreg2->getTransforms(cp); - cp = mfr2.poses; - - Eigen::Matrix4d relative_to_bg = Eigen::Matrix4d::Identity(); - if(bg->frames.size() > 0){ - relative_to_bg = cp.front().inverse()*cp.back(); - cp.pop_back(); + for(unsigned int i = 0; i < models.size(); i++){ + models[i]->fullDelete(); + delete models[i]; } - - vector masks; - for(unsigned int i = 0; i < cf.size(); i++){ - cv::Mat mask; - mask.create(cf[i]->camera->height,cf[i]->camera->width,CV_8UC1); - printf("%i -> %i %i\n",i,cf[i]->camera->height,cf[i]->camera->width); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int j = 0; j < cf[i]->camera->height*cf[i]->camera->width;j++){ - maskdata[j] = 255; - } - masks.push_back(mask); - } - - vector bgcp; - vector bgcf; - vector bgmask; - - printf("time to go!\n"); - //std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,cp,cf,masks,cp,cf,masks,true);//Determine self occlusions - - for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ - bgcp.push_back(relative_to_bg*bg->relativeposes[k]); - bgcf.push_back(bg->frames[k]); - bgmask.push_back(bg->modelmasks[k]->getMask()); - } - std::vector external_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,bgcp,bgcf,bgmask,cp,cf,masks,true);//Determine self occlusions -*/ -/* - for(unsigned int i = 0; i < cf.size(); i++){ - cv::Mat mask; - mask.create(cf[i]->camera->height,cf[i]->camera->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); - unsigned char * externalmaskdata = (unsigned char *)(external_masks[i].data); - for(unsigned int j = 0; j < cf[i]->camera->height*cf[i]->camera->width;j++){ - if(externalmaskdata[j] > 0 && internalmaskdata[j] == 0 ){maskdata[j] = 255;} - else{ - maskdata[j] = 0; - } - //maskdata[i] = std::max(internalmaskdata[i],dynamicmaskdata[i]); - } - - - cv::imshow( "rgb", cf[i]->rgb ); - cv::imshow( "externalmask", internal_masks[i] ); - cv::imshow( "externalmask", external_masks[i] ); - cv::imshow( "mask", mask ); - cv::waitKey(0); -// bg->frames.push_back(cf[i]); -// bg->relativeposes.push_back(cp[i]); -// bg->modelmasks.push_back(new reglib::ModelMask(mask)); - } -*/ -/* - std::vector internal_masks = mu->computeDynamicObject(bg,relative_to_bg,cp,cf,masks);//Determine self occlusions - // std::vector dynamic_masks = mu->computeDynamicObject(bg,relative_to_bg,cp2,cf2,internal_masks);//Determine occlusion of background occlusions - for(unsigned int i = 0; i < cf.size(); i++){ - - cv::Mat mask; - mask.create(cf[i]->camera->height,cf[i]->camera->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); - //unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[i].data); - for(unsigned int i = 0; i < cf[i]->camera->height*cf[i]->camera->width;i++){ - //maskdata[i] = std::max(internalmaskdata[i],dynamicmaskdata[i]); - } - - bg->frames.push_back(cf[i]); - bg->relativeposes.push_back(cp[i]); - bg->modelmasks.push_back(new reglib::ModelMask(mask)); - } -*/ + models.clear(); return true; } @@ -431,103 +277,4 @@ int main(int argc, char** argv){ ROS_INFO("Ready to add use segment_model."); ros::spin(); - -/* -exit(0); - viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0.5, 0, 0.5); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); - - ros::NodeHandle pn("~"); - for(int ar = 1; ar < argc; ar++){ - string overall_folder = std::string(argv[ar]); - - vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); - printf("sweep_xmls\n"); - for (auto sweep_xml : sweep_xmls) { - printf("sweep_xml: %s\n",sweep_xml.c_str()); - load2(sweep_xml); - } - } - - for(unsigned int i = 0; i < models.size(); i++){ - printf("%i -> %i\n",i,models[i]->frames.size()); - - vector cp; - vector cf; - vector mm; - - vector cp_front; - vector cf_front; - vector mm_front; - for(int j = 0; j <= i; j++){ - cp_front.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); - cf_front.push_back(models[j]->frames.front()); - mm_front.push_back(models[j]->modelmasks.front()); - } - - if(i > 0){ - reglib::MassRegistrationPPR2 * massreg = new reglib::MassRegistrationPPR2(0.05); - massreg->timeout = 1200; - massreg->viewer = viewer; - massreg->visualizationLvl = 0; - - massreg->maskstep = 5;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massreg->nomaskstep = 5;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massreg->nomask = true; - massreg->stopval = 0.0005; - - massreg->setData(cf_front,mm_front); - - - reglib::MassFusionResults mfr_front = massreg->getTransforms(cp_front); - - for(int j = i; j >= 0; j--){ - Eigen::Matrix4d change = mfr_front.poses[j] * cp_front[j].inverse(); - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); - cf.push_back(models[j]->frames[k]); - mm.push_back(models[j]->modelmasks[k]); - } - } - }else{ - for(int j = i; j >= 0; j--){ - Eigen::Matrix4d change = Eigen::Matrix4d::Identity();//mfr_front.poses[j] * cp_front[j].inverse(); - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); - cf.push_back(models[j]->frames[k]); - mm.push_back(models[j]->modelmasks[k]); - } - } - } - - reglib::MassRegistrationPPR2 * massreg2 = new reglib::MassRegistrationPPR2(0.0); - massreg2->timeout = 1200; - massreg2->viewer = viewer; - massreg2->visualizationLvl = 1; - - massreg2->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massreg2->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massreg2->nomask = true; - massreg2->stopval = 0.0005; - - massreg2->setData(cf,mm); - reglib::MassFusionResults mfr2 = massreg2->getTransforms(cp); - cp = mfr2.poses; - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models[i], reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; - - vector mats; - mu->computeDynamicObject(cp,cf,mats); - - //delete mu; - } - - - */ } diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index 622145c3..ebd16f0b 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -125,7 +125,7 @@ reglib::Model * load2(std::string sweep_xml){ reglib::Model * sweepmodel = 0; std::vector current_room_frames; - for (size_t i=0; i < 2 && irecomputeModelPoints(); printf("nr points: %i\n",sweepmodel->points.size()); -// sweepmodel->fullDelete(); -// delete sweepmodel; - models.push_back(sweepmodel); return sweepmodel; } -void runExitBehaviour(){ -// for(int j = 0; j < models.size(); j++){ -// models[j]->fullDelete(); -// } - printf("done\n"); - viewer.reset(); -} - int main(int argc, char** argv){ reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); -// viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); -// viewer->setBackgroundColor (0.5, 0, 0.5); -// viewer->addCoordinateSystem (1.0); -// viewer->initCameraParameters (); + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); for(int ar = 1; ar < argc; ar++){ string overall_folder = std::string(argv[ar]); @@ -207,7 +194,7 @@ int main(int argc, char** argv){ } } -//Not needed if metaroom well calibrated + //Not needed if metaroom well calibrated reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); bgmassreg->timeout = 20; bgmassreg->viewer = viewer; @@ -223,45 +210,14 @@ int main(int argc, char** argv){ delete bgmassreg; -// reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); -// mu->occlusion_penalty = 15; -// mu->massreg_timeout = 60*4; -// mu->viewer = viewer; - for(size_t j = 0; j < models.size(); j++){ -// //models[j]->relativeposes = bgmfr.poses; -// models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - } - - - - - - - -ros::init(argc, argv, "test_segment"); -ros::NodeHandle n; -ros::ServiceClient segmentation_client = n.serviceClient("segment_model"); - - - - - - - - -// - - -// for(int i = 0; i < models.front()->relativeposes.size(); i++){ -// models.front()->frames[i]->camera->print(); -// std::cout << models.front()->relativeposes[i] << std::endl << std::endl; -// } - + ros::init(argc, argv, "test_segment"); + ros::NodeHandle n; + ros::ServiceClient segmentation_client = n.serviceClient("segment_model"); for(unsigned int i = 1; i < models.size(); i++){ quasimodo_msgs::segment_model sm; sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); - /* + if(i > 0){ sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]); } @@ -270,127 +226,14 @@ ros::ServiceClient segmentation_client = n.serviceClientfullDelete(); delete models[j]; } - return 0; - -// mff.request.mask = *(maskBridgeImage.toImageMsg()); -// mff.request.isnewmodel = (j == (fadded.size()-1)); -// mff.request.frame_id = fid[j]; - -// if (model_from_frame_client.call(mff)){//Build model from frame -// int model_id = mff.response.model_id; -// if(model_id > 0){ -// ROS_INFO("model_id%i", model_id ); -// } -// }else{ROS_ERROR("Failed to call service index_frame");} -/* - ros::NodeHandle pn("~"); - - - for(unsigned int i = 0; i < models.size(); i++){ - printf("%i -> %i\n",i,models[i]->frames.size()); - - vector cp; - vector cf; - vector mm; - - vector cp_front; - vector cf_front; - vector mm_front; - for(int j = 0; j <= i; j++){ - cp_front.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); - cf_front.push_back(models[j]->frames.front()); - mm_front.push_back(models[j]->modelmasks.front()); - } - - if(i > 0){ - reglib::MassRegistrationPPR2 * massreg = new reglib::MassRegistrationPPR2(0.05); - massreg->timeout = 1200; - massreg->viewer = viewer; - massreg->visualizationLvl = 0; - - massreg->maskstep = 5;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massreg->nomaskstep = 5;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massreg->nomask = true; - massreg->stopval = 0.0005; - - // massreg->setData(models[i]->frames,models[i]->modelmasks); - // reglib::MassFusionResults mfr = massreg->getTransforms(models[i]->relativeposes); - - massreg->setData(cf_front,mm_front); - - - reglib::MassFusionResults mfr_front = massreg->getTransforms(cp_front); - - for(int j = i; j >= 0; j--){ - Eigen::Matrix4d change = mfr_front.poses[j] * cp_front[j].inverse(); - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); - cf.push_back(models[j]->frames[k]); - mm.push_back(models[j]->modelmasks[k]); - } - } - }else{ - for(int j = i; j >= 0; j--){ - Eigen::Matrix4d change = Eigen::Matrix4d::Identity();//mfr_front.poses[j] * cp_front[j].inverse(); - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - cp.push_back(change * models.front()->relativeposes.front().inverse() * models[j]->relativeposes[k]); - cf.push_back(models[j]->frames[k]); - mm.push_back(models[j]->modelmasks[k]); - } - } - } - - - - - reglib::MassRegistrationPPR2 * massreg2 = new reglib::MassRegistrationPPR2(0.0); - massreg2->timeout = 1200; - massreg2->viewer = viewer; - massreg2->visualizationLvl = 1; - - massreg2->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massreg2->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massreg2->nomask = true; - massreg2->stopval = 0.0005; - - massreg2->setData(cf,mm); - reglib::MassFusionResults mfr2 = massreg2->getTransforms(cp); - cp = mfr2.poses; - - //massreg->setData(models[i]->frames,models[i]->modelmasks); - //reglib::MassFusionResults mfr = massreg->getTransforms(models[i]->relativeposes); - -// massreg->setData(cf,mm); -// reglib::MassFusionResults mfr = massreg->getTransforms(cp); -// cp = mfr.poses; - - - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models[i], reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; - - vector mats; - mu->computeDynamicObject(cp,cf,mats);//models[i]->relativeposes, models[i]->frames, mats ); - -// models[i]->print(); -// mu->show_init_lvl = 2; -// mu->makeInitialSetup(); -// models[i]->print(); - - //delete mu; - } -*/ - ros::spin(); + printf("done\n"); + return 0; } diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index c6f1b8a9..fc3d3421 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -33,7 +33,6 @@ Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){ } void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask){ - printf("addSuperPoints\n"); bool * maskvec = modelmask->maskvec; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); unsigned short * depthdata = (unsigned short *)(frame->depth.data); @@ -66,14 +65,15 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr const float ifx = 1.0/camera->fx; const float ify = 1.0/camera->fy; - bool * isfused = new bool[width*height]; + std::vector isfused; + isfused.resize(width*height); + + //bool * isfused = new bool[width*height]; for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} -printf("%i\n",__LINE__); + + printf("wh:%i %i\n",width,height); for(unsigned int ind = 0; ind < spvec.size();ind++){ - if(ind % 1000 == 0){ - //printf("ind: %i\n",ind); - } superpoint & sp = spvec[ind]; float src_x = sp.point(0); @@ -150,7 +150,7 @@ printf("%i\n",__LINE__); } } } -printf("%i\n",__LINE__); + int nr_fused = 0; int nr_mask = 0; for(unsigned int w = 0; w < width; w++){ @@ -160,7 +160,6 @@ printf("%i\n",__LINE__); nr_mask += maskvec[ind] > 0; } } -printf("%i\n",__LINE__); for(unsigned int w = 0; w < width; w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; @@ -197,8 +196,7 @@ printf("%i\n",__LINE__); } } } -printf("%i\n",__LINE__); - delete[] isfused; + //delete[] isfused; } void Model::showHistory(boost::shared_ptr viewer){ @@ -365,7 +363,6 @@ std::vector::Ptr> ret; } void Model::addAllSuperPoints(std::vector & spvec, Eigen::Matrix4d pose){ - printf("addAllSuperPoints\n"); for(unsigned int i = 0; i < frames.size(); i++){ addSuperPoints(spvec, pose*relativeposes[i], frames[i], modelmasks[i]); } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 62399557..fc6d37ab 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -778,7 +778,10 @@ void ModelUpdater::addSuperPoints(vector & spvec, Matrix4d p, RGBDFr const float ifx = 1.0/camera->fx; const float ify = 1.0/camera->fy; - bool * isfused = new bool[width*height]; + //bool * isfused = new bool[width*height]; + std::vector isfused; + printf("wh: %i %i -> %i\n",width,height,width*height); + isfused.resize(width*height); for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} @@ -907,7 +910,7 @@ void ModelUpdater::addSuperPoints(vector & spvec, Matrix4d p, RGBDFr } } - delete[] isfused; + //delete[] isfused; if(debugg){ pcl::PointCloud::Ptr cloud = getPCLnormalcloud(spvec); @@ -1951,23 +1954,50 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int int other2 = ind+2*dir; int other = ind+dir; + float z3 = idepth*float(depthdata[other2]); float z2 = idepth*float(depthdata[other]); - if(z3 > 0){z2 = 2*z2-z3;} - if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb((z-z2)/(z*z+z2*z2));} + float dz = fabs(z-z2); + + //if(z3 > 0){z2 = 2*z2-z3;} + if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} + + //dz = fabs(z-z2); + //dz = std::min(dz,fabs(z-z2)); + + if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} } if(h > 1){ int dir = -width; + int other2 = ind+2*dir; int other = ind+dir; float z3 = idepth*float(depthdata[other2]); float z2 = idepth*float(depthdata[other]); - if(z3 > 0){z2 = 2*z2-z3;} - if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb((z-z2)/(z*z+z2*z2));} + float dz = fabs(z-z2); + + //if(z3 > 0){z2 = 2*z2-z3;} + //if(z3 > 0){dz = std::min(dz,fabs(z- 2*z2-z3));} + + + //dz = std::min(dz,fabs(z-z2)); + if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} + if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + +// int other2 = ind+2*dir; +// int other = ind+dir; + +// float z3 = idepth*float(depthdata[other2]); +// float z2 = idepth*float(depthdata[other]); +// if(z3 > 0){z2 = 2*z2-z3;} + +// float dz = z-z2; + +// if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} } } } @@ -2006,6 +2036,8 @@ std::vector doInference(std::vector & prior, std::vector< std::vect for(unsigned int j = 0; j < connectionId[i].size();j++){ double weight = connectionStrength[i][j]; g -> add_edge( i, connectionId[i][j], weight, weight ); + //try{g -> add_edge( i, connectionId[i][j], weight, weight );} + //catch(std::exception ex){printf("%i %i -> weight: %20.20f\n",i,connectionId[i],weight);exit(0);} } } @@ -2025,11 +2057,9 @@ std::vector doInference(std::vector & prior, std::vector< std::vect vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg){ std::vector newmasks; - - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - std::vector unaryprobs; double maxprob = 0.7; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); pcl::PointCloud::Ptr cloud1 (new pcl::PointCloud); pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); @@ -2108,18 +2138,8 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector *g = new gc::Graph( nr_data, nr_edges); - -// printf("nr data: %i nr edges: %i\n",nr_data, nr_edges); - - -// gc::Graph * g2 = new gc::Graph(nr_data,nr_edges); - - for(unsigned int j = 0; j < nr_data;j++){ -// g -> add_node(); + for(unsigned int j = 0; j < nr_data;j++){ if(depthdata[j] == 0){ -// g -> add_tweights(j,0,0); frame_prior[j] = -1; prior[offset+j] = -1; continue; @@ -2134,33 +2154,14 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector add_tweights( j, weightFG, weightBG ); } -//for(unsigned int j = 0; j < nr_data;j++){ -// g2 -> add_node(); -// double p_fg = frame_prior[j]; -// if(p_fg < 0){ -// g -> add_tweights( j, 0, 0 ); -// continue; -// } -// double p_bg = 1-p_fg; -// double weightFG = -log(p_fg); -// double weightBG = -log(p_bg); -// g2 -> add_tweights( j, weightFG, weightBG ); -//} - - double maxprob_same = 0.999999999; for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ + for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; - if(w > 0){ + if(w > 0 && w < width-1){ double ax = 0.5; double bx = 0.5; for(unsigned int p = 0; p < probs.size(); p+=2){ @@ -2170,22 +2171,20 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); - frame_connectionId[ind].push_back(other); - frame_connectionStrength[ind].push_back(weight); - - connectionId[ind+offset].push_back(other+offset); - connectionStrength[ind+offset].push_back(weight); - -// g -> add_edge( ind, other, weight, weight ); - //g2 -> add_edge( ind, other, weight, weight ); + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } } - if(h > 0){ + if(h > 0 && h < height-1){ double ay = 0.5; double by = 0.5; @@ -2196,42 +2195,28 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); - connectionId[ind+offset].push_back(other+offset); - connectionStrength[ind+offset].push_back(weight); - -// g -> add_edge( ind, other, weight, weight ); -// g2 -> add_edge( ind, other, weight, weight ); + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } } } } -// for(unsigned int ind = 0; ind < nr_data;ind++){ -// for(unsigned int j = 0; j < frame_connectionId[ind].size();j++){ -// int other = frame_connectionId[ind][j]; -// double weight = frame_connectionStrength[ind][j]; -// g2 -> add_edge(ind, other, weight, weight ); -// } -// } - -// int flow = g -> maxflow(); -// printf("flow: %i\n",flow); - -// int flow2 = g2 -> maxflow(); -// printf("flow2: %i\n",flow2); double end_part = getTime(); printf("part time: %10.10fs\n",end_part-start_part); - std::vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); + //std::vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); Eigen::Matrix4d p = poses[i]; float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); @@ -2239,7 +2224,6 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vectoridepth_scale; const float cx = camera->cx; const float cy = camera->cy; @@ -2263,50 +2247,6 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vectorpoints.push_back(point); - - double p_fg = 0.49999; - - if(occlusions[ind] >= 1){ p_fg = maxprob;} - else if(overlaps[ind] >= 1){ p_fg = 0.4;} - - p_fg = std::max(1-maxprob,std::min(maxprob,p_fg)); - - - unaryprobs.push_back(p_fg); - } - } - } - - bool debugg1 = debugg; - if(debugg1){ - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; - if (labels[ind] == gc::Graph::SOURCE){ - point.r = 0; - point.g = 255; - point.b = 0; - }else{ - point.r = 255; - point.g = 0; - point.b = 0; - } - //cloud->points[ind] = point; - cloud1->points.push_back(point); - } } } } @@ -2361,15 +2301,6 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vectorwhat_segment(i) == gc::Graph::SOURCE)); - internaldata[i] = 255.0*((depthdata[i] == 0) || (labels[i] == gc::Graph::SOURCE)); - } - newmasks.push_back(internalmask); - //Time to compute external masks... delete[] overlaps; delete[] occlusions; @@ -2387,7 +2318,6 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector improvedglobal_labels = doInference(prior,connectionId,connectionStrength); for(unsigned int f = 0; f < frames.size(); f++){ - int offset = offsets[f]; RGBDFrame * frame = frames[f]; unsigned short * depthdata = (unsigned short *)(frame->depth.data); @@ -2397,19 +2327,16 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vectorwidth; const unsigned int height = camera->height; - unsigned int nr_pixels = frame->camera->width * frame->camera->height; cv::Mat originalmask; originalmask.create(height,width,CV_8UC1); unsigned char * originaldata = (unsigned char *)(originalmask.data); - cv::Mat improvedmask; improvedmask.create(height,width,CV_8UC1); unsigned char * improveddata = (unsigned char *)(improvedmask.data); - cv::Mat diffmask; diffmask.create(height,width,CV_8UC3); unsigned char * diffdata = (unsigned char *)(diffmask.data); @@ -2435,30 +2362,137 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vectorshow(false); - cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); - cv::imshow( "Original", originalmask ); - cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); - cv::imshow( "Improved", improvedmask ); - cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); - cv::imshow( "Difference", diffmask ); - cv::waitKey(0); - //newmasks.push_back(internalmask); +// frame->show(false); +// cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "Original", originalmask ); +// cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "Improved", improvedmask ); +// cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "Difference", diffmask ); +// cv::waitKey(0); + newmasks.push_back(improvedmask); } if(debugg){ + + + for(unsigned int i = 0; i < frames.size(); i++){ + int offset = offsets[i]; + RGBDFrame * frame = frames[i]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + Eigen::Matrix4d p = poses[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + unsigned int nr_pixels = frame->camera->width * frame->camera->height; + + cv::Mat originalmask; + originalmask.create(height,width,CV_8UC1); + unsigned char * originaldata = (unsigned char *)(originalmask.data); + + cv::Mat improvedmask; + improvedmask.create(height,width,CV_8UC1); + unsigned char * improveddata = (unsigned char *)(improvedmask.data); + + cv::Mat diffmask; + diffmask.create(height,width,CV_8UC3); + unsigned char * diffdata = (unsigned char *)(diffmask.data); + + for(unsigned int i = 0; i < nr_pixels;i++){ + //internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); + originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph::SOURCE)); + improveddata[i] = 255.0*((depthdata[i] == 0) || (improvedglobal_labels[i+offset] == gc::Graph::SOURCE)); + if(improveddata[i] < originaldata[i]){ + diffdata[3*i+0] = 0; + diffdata[3*i+1] = 255; + diffdata[3*i+2] = 0; + }else if(improveddata[i] > originaldata[i]){ + diffdata[3*i+0] = 0; + diffdata[3*i+1] = 0; + diffdata[3*i+2] = 255; + }else{ + diffdata[3*i+0] = 0; + diffdata[3*i+1] = 0; + diffdata[3*i+2] = 0; + } + } + +// frame->show(false); +// cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "Original", originalmask ); +// cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "Improved", improvedmask ); +// cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "Difference", diffmask ); +// cv::waitKey(0); + + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + + //(improvedglobal_labels[i+offset] == gc::Graph::SOURCE) + + //if (labels[ind] == gc::Graph::SOURCE){ + //if (improvedglobal_labels[ind+offset] == gc::Graph::SOURCE){ + if(improveddata[ind] == 0){ + point.r = 255; + point.g = 0; + point.b = 0; + }else{ + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; + } + cloud1->points.push_back(point); + } + } + } + + + } + viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + viewer->addPointCloud (cloud1, pcl::visualization::PointCloudColorHandlerRGBField(cloud1), "scloud"); viewer->spin(); viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud1, pcl::visualization::PointCloudColorHandlerRGBField(cloud1), "scloud"); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); viewer->spin(); viewer->removeAllPointClouds(); viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "scloud"); viewer->spin(); + + viewer->removeAllPointClouds(); } return newmasks; From 0ed58f131dffce0250b4ef6ffdeadb7a985bfeba Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 10 Aug 2016 07:35:14 +0200 Subject: [PATCH 054/255] start of bilateral filter creation --- .../quasimodo_models/src/core/RGBDFrame.cpp | 215 ++++++++++++++++++ 1 file changed, 215 insertions(+) diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 707c0709..fe90c590 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -173,6 +173,221 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt */ + if(true){ + + cv::Mat colimg_est; + colimg_est.create(height,width,CV_32FC3); + float * cest = (float *)colimg_est.data; + + cv::Mat colimg_base; + colimg_base.create(height,width,CV_32FC3); + float * cbase = (float *)colimg_base.data; + + cv::Mat cn_est; + cn_est.create(height,width,CV_32FC3); + float * cnest = (float *)cn_est.data; + + cv::Mat z_est; + z_est.create(height,width,CV_32FC3); + float * zest = (float *)z_est.data; + + cv::Mat z_base; + z_base.create(height,width,CV_32FC3); + float * zbase = (float *)z_base.data; + + cv::Mat nimg; + nimg.create(height,width,CV_32FC3); + float * nest = (float *)nimg.data; + + + cv::Mat dz_est; + dz_est.create(height,width,CV_32FC2); + float * dzest = (float *)dz_est.data; + + for(int i = 0; i < width*height; i++){ + zbase[i] = zest[i] = idepth*double(depthdata[i]); + } + + double w0 = 0.1; + for(int w = 90; w < 1110; w++){ + for(int h = 0; h < height;h++){ + int i = h*width+w; + + if( h == 100){ + printf("%i: base: %5.5f %5.5f\n",w,zbase[i],zest[i]); + } + } + } + + for(int i = 0; i < 3*width*height; i++){ + cbase[i] = cest[i] = float(rgbdata[i])/256.0; + } + + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int i = h*width+w; + dzest[2*i+0] = 0; + dzest[2*i+1] = 0; +// double z = zest[i]; +// if(w < width-1){ +// dzest[2*i+0] = z-zest[i+1]; +// }else{ +// dzest[2*i+0] = dzest[2*i-2]; +// } + +// if(h < height-1){ +// dzest[2*i+1] = z-zest[i+width]; +// }else{ +// dzest[2*i+1] = dzest[2*(i-width)+1]; +// } + } + } + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(width*height); + + boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0, 0, 0); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + for(int it = 0; true; it++){ + printf("it: %i\n",it); + if(it % 100 == 0){ + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::PointXYZRGBA & p = cloud->points[ind]; + p.b = cest[3*ind+0]*256.0; + p.g = cest[3*ind+1]*256.0; + p.r = cest[3*ind+2]*256.0; + double z = zest[ind]; + if(z > 0){ + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + }else{ + p.x = 0; + p.y = 0; + p.z = 0; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } + +//// Compute normals +// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", colimg_est ); +// cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); cv::imshow( "normals", nimg ); +// cv::namedWindow( "dz", cv::WINDOW_AUTOSIZE ); cv::imshow( "dz", dz_est ); +// cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", z_est); +// cv::waitKey(0); + +// for(int w = 0; w < width; w++){ +// for(int h = 0; h < height;h++){ +// int i = h*width+w; +// double z = zest[i]; +// if(w < width-1){ dzest[2*i+0] = z-zest[i+1];} +// else{ dzest[2*i+0] = dzest[2*i-2];} + +// if(h < height-1){ dzest[2*i+1] = z-zest[i+width];} +// else{ dzest[2*i+1] = dzest[2*(i-width)+1];} +// } +// } + + double w0 = 0.001; + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int i = h*width+w; + + if(w < 103 && w > 97 && h == 100){ + printf("mid %i: base: %5.5f %5.5f\n",w,zbase[i],zest[i]); + } + double z = zbase[i]; + double sumw = w0/(z*z); + double sumz = sumw*zbase[i]; + + if(z == 0){ + sumz = 0; + sumw = 0; + } + + if(w > 0){ + int i2 = i-1; + + double predz = zest[i2]+dzest[2*i+0]; + double weight = fabs(z -predz) < 0.05; + sumz+=weight*predz; + sumw+=weight; + +// if(w == 100 && h == 100){ +// printf("left %i: base: %5.5f %5.5f\n",w-1,zbase[i2],zest[i2]); +// } + } + + if(w < width-1){ + int i2 = i+1; + double predz = zest[i2]-dzest[2*i+0]; + double weight = fabs(z -predz) < 0.05; + sumz+=weight*predz; + sumw+=weight; + } + + if(h > 0){ + int i2 = i-width; + double predz = zest[i2]+dzest[2*i+0]; + double weight = fabs(z -predz) < 0.05; + sumz+=weight*predz; + sumw+=weight; + } + + if(h < height-1){ + int i2 = i+width; + double predz = zest[i2]-dzest[2*i+0]; + double weight = fabs(z -predz) < 0.05; + sumz+=weight*predz; + sumw+=weight; + } + + if(sumw == 0){ + zest[i] = 0; + }else{ + zest[i] = sumz/sumw; + } + } + } + + + + } +// double * z_base = new double[width*height]; +// double * col_base = new double[3*width*height]; +// double * z_est = new double[width*height]; +// double * col_est = new double[3*width*height]; +// double * normal_est = new double[3*width*height]; + +// for(int i = 0; i < width*height; i++){ +// z_base[i] = z_est[i] = idepth*double(depthdata[ind]); +// col_est[3*i+0] = rgbdata[3*i+0]; +// col_est[3*i+1] = rgbdata[3*i+1]; +// col_est[3*i+2] = rgbdata[3*i+2]; +// } +// delete[] z_base; +// delete[] col_base; +// delete[] z_est; +// delete[] col_est; +// delete[] normal_est; + + + + + } + //printf("%s LINE:%i\n",__FILE__,__LINE__); if(compute_normals){ normals.create(height,width,CV_32FC3); From a98b843817d9f1bfa492142225e440f6196fbf65 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Wed, 10 Aug 2016 13:41:24 +0200 Subject: [PATCH 055/255] Fixed major issues to search for johan's data base models, testing remains --- .../dynamic_retrieval.h | 62 +++++-- .../msg/fused_world_state_object.msg | 3 +- quasimodo/quasimodo_msgs/msg/model.msg | 1 + quasimodo/quasimodo_msgs/srv/insert_model.srv | 8 + .../scripts/insert_object_server.py | 160 ++++++++++++++++++ 5 files changed, 222 insertions(+), 12 deletions(-) create mode 100644 quasimodo/quasimodo_msgs/srv/insert_model.srv create mode 100755 quasimodo/quasimodo_object_search/scripts/insert_object_server.py diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index 216f9f03..a85edad5 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -46,8 +46,19 @@ struct path_result > { using type = std::vector; }; +enum uri_kind { + metaroom, + mongodb, + all +}; + +// this should have some switch for what to keep +// if kind = uri_kind::mongodb, every index will be returned but we will only keep the first (what number) which are valid +// alternatively, maybe return empty paths for everything else? sounds good, or rather, let's still return the files +// but somehow we need to know how many to load... let's hardcode for now template -std::vector get_retrieved_paths(const std::vector& scores, const vocabulary_summary& summary, bool verbose = false) +std::vector get_retrieved_paths(const std::vector& scores, const vocabulary_summary& summary, + bool verbose = false, uri_kind kind = uri_kind::all) { if (verbose) { std::cout << "Entering get_retrieved_paths" << std::endl; @@ -89,6 +100,7 @@ std::vector get_retrieved_paths(const std::vector retrieved_paths; + size_t loaded_mongodb_results = 0; for (IndexT s : scores) { // should we write to some temporary files and return the paths to those? // e.g. in ~/.ros/quasimodo @@ -108,6 +120,13 @@ std::vector get_retrieved_paths(const std::vector 20) { // TODO: hard coded for now, let's revisit + retrieved_paths.push_back(boost::filesystem::path()); + continue; + } + ++loaded_mongodb_results; + std::string db_uri = uri.substr(10, uri.size()-10); std::vector strs; boost::split(strs, db_uri, boost::is_any_of("/")); @@ -196,13 +215,13 @@ std::vector get_retrieved_paths(const std::vector get_retrieved_paths(const std::vector& scores, const vocabulary_summary& summary); std::vector::result_type> > -get_retrieved_path_scores(const std::vector::result_type>& scores, const vocabulary_summary& summary, bool verbose = false) +get_retrieved_path_scores(const std::vector::result_type>& scores, const vocabulary_summary& summary, bool verbose = false, uri_kind kind = uri_kind::all) { if (verbose) { std::cout << "Entering get_retrieved_path_scores" << std::endl; } - std::vector paths = get_retrieved_paths(scores, summary, verbose); + std::vector paths = get_retrieved_paths(scores, summary, verbose, kind); std::vector::result_type> > path_scores; for (auto tup : dynamic_object_retrieval::zip(paths, scores)) { path_scores.push_back(std::make_pair(boost::get<0>(tup), boost::get<1>(tup))); @@ -216,7 +235,7 @@ get_retrieved_path_scores(const std::vector::result_ty } std::vector, grouped_vocabulary_tree::result_type> > -get_retrieved_path_scores(const std::vector::result_type>& scores, const vocabulary_summary& summary, bool verbose = false) +get_retrieved_path_scores(const std::vector::result_type>& scores, const vocabulary_summary& summary, bool verbose = false, uri_kind kind = uri_kind::all) { // OK, now we need to get the paths for all of the stuff within the scans instead, so basically // we need to get a subsegment_sweep__path_iterator and add all of the path corresponding to groups @@ -273,7 +292,7 @@ template std::vector::type, typename VocabularyT::result_type> > query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, VocabularyT& vt, const boost::filesystem::path& vocabulary_path, - const vocabulary_summary& summary, bool verbose = false) + const vocabulary_summary& summary, bool verbose = false, uri_kind kind = uri_kind::all) { // we need to cache the vt if we are to do this multiple times if (vt.empty()) { @@ -287,9 +306,30 @@ query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, VocabularyT& vt, // add some common methods in vt for querying, really only one! std::vector scores; - vt.query_vocabulary(scores, features, nbr_query); + if (kind == uri_kind::mongodb) { + vt.query_vocabulary(scores, features, 0); // return all results, remove some later + } + else { + vt.query_vocabulary(scores, features, nbr_query); + } + + // maybe this would be the place to remove the rotten eggs, considering scores is passed as const + using result_type = std::pair::type, typename VocabularyT::result_type>; + std::vector::type, typename VocabularyT::result_type> > results = get_retrieved_path_scores(scores, summary, verbose, kind); + results.erase(std::remove_if(results.begin(), results.end(), [&kind](const result_type& r) { + if (r.first.empty()) { + return true; + } + boost::filesystem::path segment_path = r.first; + std::string name = segment_path.stem().string(); + if (kind == uri_kind::mongodb && name != "segment") { // for the mongodb results + return true; + } + return false; + }), results.end()); + results.resize(std::min(nbr_query, results.size())); - return get_retrieved_path_scores(scores, summary, verbose); + return results; } // OK, the solution here is to turn the groups into the path scores @@ -297,7 +337,7 @@ template <> std::vector >::type, typename grouped_vocabulary_tree::result_type> > query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, grouped_vocabulary_tree& vt, const boost::filesystem::path& vocabulary_path, - const vocabulary_summary& summary, bool verbose) + const vocabulary_summary& summary, bool verbose, uri_kind kind) { // we need to cache the vt if we are to do this multiple times if (vt.empty()) { @@ -310,7 +350,7 @@ query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, grouped_vocabulary std::vector::result_type> scores; vt.query_vocabulary(scores, features, nbr_query); - return get_retrieved_path_scores(scores, summary); + return get_retrieved_path_scores(scores, summary, verbose, kind); } void insert_index_score(std::vector >& weighted_indices, const vocabulary_tree::result_type& index, float score) @@ -622,7 +662,7 @@ std::pair::type, typenam std::vector::type, typename VocabularyT::result_type> > > query_reweight_vocabulary(VocabularyT& vt, HistCloudT::Ptr& query_features, size_t nbr_query, const boost::filesystem::path& vocabulary_path, - const vocabulary_summary& summary, bool verbose = false) + const vocabulary_summary& summary, bool verbose = false, uri_kind kind = uri_kind::all) { using result_type = std::vector::type, typename VocabularyT::result_type> >; @@ -637,7 +677,7 @@ query_reweight_vocabulary(VocabularyT& vt, HistCloudT::Ptr& query_features, std::cout << "Querying vocabulary..." << std::endl; TICK("query_vocabulary"); - result_type retrieved_paths = query_vocabulary(query_features, nbr_query, vt, vocabulary_path, summary, verbose); + result_type retrieved_paths = query_vocabulary(query_features, nbr_query, vt, vocabulary_path, summary, verbose, kind); TOCK("query_vocabulary"); return make_pair(retrieved_paths, result_type()); diff --git a/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg index 2733819c..02c33b4b 100644 --- a/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg +++ b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg @@ -7,4 +7,5 @@ sensor_msgs/Image[] images sensor_msgs/Image[] depths sensor_msgs/Image[] masks geometry_msgs/Pose[] transforms - +string inserted_at +string removed_at diff --git a/quasimodo/quasimodo_msgs/msg/model.msg b/quasimodo/quasimodo_msgs/msg/model.msg index c732cc22..72ad70a4 100644 --- a/quasimodo/quasimodo_msgs/msg/model.msg +++ b/quasimodo/quasimodo_msgs/msg/model.msg @@ -2,3 +2,4 @@ uint64 model_id geometry_msgs/Pose[] local_poses rgbd_frame[] frames sensor_msgs/Image[] masks +sensor_msgs/PointCloud2[] clouds diff --git a/quasimodo/quasimodo_msgs/srv/insert_model.srv b/quasimodo/quasimodo_msgs/srv/insert_model.srv new file mode 100644 index 00000000..cee7f313 --- /dev/null +++ b/quasimodo/quasimodo_msgs/srv/insert_model.srv @@ -0,0 +1,8 @@ +uint64 INSERT=1 +uint64 REMOVE=2 +uint64 action +quasimodo_msgs/model model +uint64 object_id +--- +uint64 vocabulary_id +uint64 object_id diff --git a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py new file mode 100755 index 00000000..55b95426 --- /dev/null +++ b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python + +import roslib +import rospy +from sensor_msgs.msg import PointCloud2, PointField +#from world_modeling.srv import * +from cv_bridge import CvBridge, CvBridgeError +import cv2 + +# SOMA2 stuff +from soma2_msgs.msg import SOMA2Object +from mongodb_store.message_store import MessageStoreProxy +from soma_manager.srv import * +from geometry_msgs.msg import Pose, Transform +from observation_registration_services.srv import ProcessRegisteredViews, ProcessRegisteredViewsRequest, ProcessRegisteredViewsResponse +from soma_io.state import World, Object +from datetime import datetime, timedelta +from quasimodo_msgs.msg import fused_world_state_object +from quasimodo_msgs.srv import transform_cloud, transform_cloudRequest, transform_cloudResponse +from quasimodo_msgs.srv import index_cloud, index_cloudRequest, index_cloudResponse +from quasimodo_msgs.srv import mask_pointclouds, mask_pointcloudsRequest, mask_pointcloudsResponse +from quasimodo_msgs.srv import insert_model, insert_modelRequest, insert_modelResponse +from geometry_msgs.msg import Pose +import time + +def insert_model(): + msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') + + transforms = [] + req = mask_pointcloudsRequest() + for cloud, mask, pose in zip(req.model.clouds, req.model.masks, req.model.local_poses): + req.clouds.append(cloud) + req.masks.append(mask) + + transform = Transform() + transform.rotation.x = pose.quaternion.x + transform.rotation.y = pose.quaternion.y + transform.rotation.z = pose.quaternion.z + transform.rotation.w = pose.quaternion.w + transform.translation.x = pose.position.x + transform.translation.y = pose.position.y + transform.translation.z = pose.position.z + transforms.append(transform) + + print "Waiting for retrieval_segmentation_service..." + rospy.wait_for_service('retrieval_segmentation_service') + try: + surfelize_server = rospy.ServiceProxy('retrieval_segmentation_service', mask_pointclouds) + resp = surfelize_server(req) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + return False + + req = ProcessRegisteredViewsRequest() + req.registered_views = resp.clouds + req.registered_views_transforms = transforms + + print "Waiting for surfelize_server..." + rospy.wait_for_service('surfelize_server') + try: + surfelize_server = rospy.ServiceProxy('surfelize_server', ProcessRegisteredViews) + resp = surfelize_server(req) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + return False + + new_obj = fused_world_state_object() + new_obj.surfel_cloud = resp.processed_cloud + new_obj.object_id = x.id + now = datetime.now() + new_obj.inserted_at = now.strftime("%Y-%m-%d %H:%M:%S") + + for obs, pose in zip(wo._observations, wo._poses): + new_obj.transforms.append(Pose()) + new_obj.transforms[-1].position.x = pose.position.x + new_obj.transforms[-1].position.y = pose.position.y + new_obj.transforms[-1].position.z = pose.position.z + new_obj.transforms[-1].orientation.x = pose.quaternion.x + new_obj.transforms[-1].orientation.y = pose.quaternion.y + new_obj.transforms[-1].orientation.z = pose.quaternion.z + new_obj.transforms[-1].orientation.w = pose.quaternion.w + + print "Waiting for retrieval_features_service..." + rospy.wait_for_service('retrieval_features_service') + req = transform_cloudRequest() + req.cloud = resp.processed_cloud + try: + surfelize_server = rospy.ServiceProxy('retrieval_features_service', transform_cloud) + resp = surfelize_server(req) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + return False + + new_obj.features = resp.cloud1 + new_obj.keypoints = resp.cloud2 + + object_id = msg_store.insert(new_obj) + + print "Waiting for retrieval_vocabulary_service..." + rospy.wait_for_service('retrieval_vocabulary_service') + req = index_cloudRequest() + req.cloud = new_obj.features + req.object_id = object_id + try: + surfelize_server = rospy.ServiceProxy('retrieval_vocabulary_service', index_cloud) + resp = surfelize_server(req) + except rospy.ServiceException, e: + print "Service call failed: %s"%e + return False + + new_obj.vocabulary_id = resp.id + msg_store.update_id(object_id, new_obj) + # Now, simply save this in mongodb, or maybe extract features first? + + print object_id + + print("done") + + resp = insert_modelResponse() + resp.vocabulary_id = new_obj.vocabulary_id + resp.object_id = new_obj.object_id + + return resp + +def remove_model(req): + + msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') + # msg_store.delete(req.object_id) + # resp = insert_modelResponse() + # resp.object_id = req.object_id + + new_obj = msg_store.query_id(req.object_id, fused_world_state_object) + now = datetime.now() + new_obj.removed_at = now.strftime("%Y-%m-%d %H:%M:%S") + msg_store.update_id(req.object_id, new_obj) + + return resp + +def service_callback(req): + + if req.action == insert_modelRequest.REMOVE: + resp = remove_model(req) + if not resp: + print "Failed to remove model..." + elif req.action == insert_modelRequest.INSERT: + resp = insert_model(req) + if not resp: + print "Failed to remove model..." + else: + resp = False + print "Service options are 1: INSERT and 2: REMOVE" + + return resp + +if __name__ == '__main__': + rospy.init_node('insert_object_server', anonymous = False) + + service = rospy.Service('insert_model_service', insert_model, service_callback) + + rospy.spin() From db2b7bfed1217a2b676461f9fa9ef82a49bb6c76 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 10 Aug 2016 17:19:15 +0200 Subject: [PATCH 056/255] working on bilateral filering and adding services for segmentation on robot --- quasimodo/quasimodo_brain/CMakeLists.txt | 4 + quasimodo/quasimodo_brain/src/Util/Util.cpp | 84 ++++ quasimodo/quasimodo_brain/src/Util/Util.h | 6 + .../src/segment_room_pairs.cpp | 220 ++++++++++ .../quasimodo_brain/src/test_segment.cpp | 25 -- .../quasimodo_models/src/core/RGBDFrame.cpp | 406 ++++++++++++++++-- .../quasimodo_metaroom_segmentation.srv.txt | 4 + 7 files changed, 689 insertions(+), 60 deletions(-) create mode 100644 quasimodo/quasimodo_brain/src/segment_room_pairs.cpp create mode 100644 quasimodo/quasimodo_msgs/srv/quasimodo_metaroom_segmentation.srv.txt diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 421aa503..93fdba7c 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -144,6 +144,10 @@ add_executable( test_segment src/test_segment.cpp) add_dependencies( test_segment roscpp quasimodo_msgs_generate_messages_cpp) target_link_libraries( test_segment quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +add_executable( segment_room_pairs src/segment_room_pairs.cpp) +add_dependencies( segment_room_pairs roscpp quasimodo_msgs_generate_messages_cpp) +target_link_libraries( segment_room_pairs quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + add_executable( segmentationserver src/segmentationserver.cpp) add_dependencies( segmentationserver roscpp quasimodo_msgs_generate_messages_cpp) target_link_libraries( segmentationserver quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 9201491a..7790cc2c 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -103,4 +103,88 @@ quasimodo_msgs::model getModelMSG(reglib::Model * model){ return msg; } +std::vector getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms){ + std::vector toRet; + ifstream in(poses_file); + if (!in.is_open()){ + cout<<"ERROR: cannot find poses file "<> temp; + transform(j,k) = temp; + } + } + toRet.push_back(transform); + } + return toRet; +} + +reglib::Model * load_metaroom_model(std::string sweep_xml){ + int slash_pos = sweep_xml.find_last_of("/"); + std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; + printf("folder: %s\n",sweep_folder.c_str()); + + SimpleXMLParser parser; + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); + + reglib::Model * sweepmodel = 0; + + std::vector current_room_frames; + for (size_t i=0; ifx = 532.158936; + cam->fy = 533.819214; + cam->cx = 310.514310; + cam->cy = 236.842039; + + + cout<<"Intermediate cloud size "<points.size()<frames.push_back(frame); + sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose); + sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); + } + } + + //sweepmodel->recomputeModelPoints(); + printf("nr points: %i\n",sweepmodel->points.size()); + + return sweepmodel; +} + } diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index c5739441..a8cff3a5 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -30,6 +30,8 @@ #include "modelupdater/ModelUpdater.h" #include "core/RGBDFrame.h" +#include "metaroom_xml_parser/simple_xml_parser.h" + namespace quasimodo_brain { double getTime(); @@ -38,6 +40,10 @@ reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg); void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp = Eigen::Affine3d::Identity()); quasimodo_msgs::model getModelMSG(reglib::Model * model); +std::vector getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms); + +reglib::Model * load_metaroom_model(std::string sweep_xml); + } #endif diff --git a/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp new file mode 100644 index 00000000..db503095 --- /dev/null +++ b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp @@ -0,0 +1,220 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include +#include + +#include +#include + +#include "eigen_conversions/eigen_msg.h" +#include "tf_conversions/tf_eigen.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" +#include "metaroom_xml_parser/simple_summary_parser.h" + +#include + +#include "ros/ros.h" +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" +#include "quasimodo_msgs/segment_model.h" + +#include "ros/ros.h" +#include +#include +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" + +#include +#include + +#include "modelupdater/ModelUpdater.h" +#include "core/RGBDFrame.h" +#include "Util/Util.h" + + + +using namespace std; + +//typedef pcl::PointXYZRGB PointType; +//typedef pcl::PointCloud Cloud; +//typedef typename Cloud::Ptr CloudPtr; +//typedef pcl::search::KdTree Tree; +//typedef semantic_map_load_utilties::DynamicObjectData ObjectData; + +//using pcl::visualization::PointCloudColorHandlerCustom; + +//std::vector< std::vector > rgbs; +//std::vector< std::vector > depths; +//std::vector< std::vector > masks; +//std::vector< std::vector >tfs; +//std::vector< std::vector > initposes; +//std::vector< std::vector< image_geometry::PinholeCameraModel > > cams; + +//pcl::visualization::PCLVisualizer* pg; +//boost::shared_ptr viewer; + +//std::vector models; +/* +reglib::Model * load2(std::string sweep_xml){ + int slash_pos = sweep_xml.find_last_of("/"); + std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; + printf("folder: %s\n",sweep_folder.c_str()); + + SimpleXMLParser parser; + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); + +// projection +// 532.158936 0.000000 310.514310 0.000000 +// 0.000000 533.819214 236.842039 0.000000 +// 0.000000 0.000000 1.000000 0.000000 + + reglib::Model * sweepmodel = 0; + + std::vector current_room_frames; + for (size_t i=0; ifx = 532.158936; + cam->fy = 533.819214; + cam->cx = 310.514310; + cam->cy = 236.842039; + + + cout<<"Intermediate cloud size "<points.size()<frames.push_back(frame); + sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose); + sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); + } + } + + //sweepmodel->recomputeModelPoints(); + printf("nr points: %i\n",sweepmodel->points.size()); + + models.push_back(sweepmodel); + return sweepmodel; +} +*/ +int main(int argc, char** argv){ + ros::init(argc, argv, "segmentationserver_metaroom"); + ros::NodeHandle n; +// ros::ServiceServer service = n.advertiseService("segment_model", segment_model); + ros::spin(); + /* + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + for(int ar = 1; ar < argc; ar++){ + string overall_folder = std::string(argv[ar]); + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + printf("sweep_xmls\n"); + for (auto sweep_xml : sweep_xmls) { + printf("sweep_xml: %s\n",sweep_xml.c_str()); + load2(sweep_xml); + } + } + + //Not needed if metaroom well calibrated + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); + bgmassreg->timeout = 20; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = false; + bgmassreg->visualizationLvl = 0; + bgmassreg->maskstep = 10; + bgmassreg->nomaskstep = 10; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->setData(models.front()->frames,models.front()->modelmasks); + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); + delete bgmassreg; + + + ros::init(argc, argv, "test_segment"); + ros::NodeHandle n; + ros::ServiceClient segmentation_client = n.serviceClient("segment_model"); + + for(unsigned int i = 1; i < models.size(); i++){ + quasimodo_msgs::segment_model sm; + sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); + + if(i > 0){ + sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]); + } + + if (segmentation_client.call(sm)){//Build model from frame + //int model_id = mff.response.model_id; + printf("segmented: %i\n",i); + }else{ROS_ERROR("Failed to call service segment_model");} + } + //ros::spin(); + + delete reg; + for(size_t j = 0; j < models.size(); j++){ + models[j]->fullDelete(); + delete models[j]; + } + printf("done\n"); + */ + return 0; +} diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index ebd16f0b..e827cede 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -82,31 +82,6 @@ std::vector< std::vector< image_geometry::PinholeCameraModel > > cams; pcl::visualization::PCLVisualizer* pg; boost::shared_ptr viewer; - - -std::vector getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms){ - std::vector toRet; - ifstream in(poses_file); - if (!in.is_open()){ - cout<<"ERROR: cannot find poses file "<> temp; - transform(j,k) = temp; - } - } - toRet.push_back(transform); - } - return toRet; -} - std::vector models; reglib::Model * load2(std::string sweep_xml){ diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index fe90c590..95954416 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -54,6 +54,23 @@ RGBDFrame::RGBDFrame(){ bool updated = true; void on_trackbar( int, void* ){updated = true;} +//void update(cv::Mat & err, cv::Mat & est, cv::Mat & dx, int dir){ +//} + +float pred(float targetW, float targetH, int sourceW, int sourceH, int width, float * est, float * dx, float * dy){ + int ind = sourceH * width + sourceW; + return est[ind]+(targetW-float(sourceW))*dx[ind]+(targetH-float(sourceH))*dy[ind]; +} + +float weightFunc(int w0, int h0, int w1, int h1, int width, std::vector & est, std::vector & dx, std::vector & dy){ + float weight = 1; + for(int c = 0; c < est.size(); c++){ + float error = ((float*)(est[c].data))[h0*width+w0] - pred(w0,h0,w1,h1,width, (float*)(est[c].data), (float*)(dx[c].data), (float*)(dy[c].data)); + weight *= exp(-0.5*error*error/(0.1*0.1)); + } + return weight; +} + RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals){ keyval = ""; @@ -173,7 +190,357 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt */ - if(true){ + if(false){ + //Per channel + //Base + //est + //dx + //dy + //errX + //errY + std::vector channels_base; + std::vector channels_noise; + std::vector channels_est; + std::vector channels_dx; + std::vector channels_dy; + std::vector channels_errx; + std::vector channels_erry; + + cv::Mat z_base; + z_base.create(height,width,CV_32FC1); + float * zbase = (float *)z_base.data; + + for(int i = 0; i < width*height; i++){ + zbase[i] = idepth*double(depthdata[i]); + } + + channels_base.push_back(z_base); + for(int c = 0; c < 3; c++){ + cv::Mat current_base; + current_base.create(height,width,CV_32FC1); + float * currentbase = (float *)current_base.data; + + for(int i = 0; i < width*height; i++){ + currentbase[i] = float(rgbdata[3*i+c])/256.0; + } + channels_base.push_back(current_base); + } + + for(int c = 0; c < channels_base.size(); c++){ + float * data = (float *)(channels_base[c].data); + cv::Mat tmp; + tmp.create(height,width,CV_32FC1); + float * tmpdata = (float *)tmp.data; + for(int i = 0; i < width*height; i++){ + tmpdata[i] = data[i]; + } + channels_est.push_back(tmp); + } + + for(int c = 0; c < channels_base.size(); c++){ + cv::Mat tmp; + tmp.create(height,width,CV_32FC1); + float * tmpdata = (float *)tmp.data; + for(int i = 0; i < width*height; i++){ + tmpdata[i] = 0; + } + channels_dx.push_back(tmp); + } + + for(int c = 0; c < channels_base.size(); c++){ + cv::Mat tmp; + tmp.create(height,width,CV_32FC1); + float * tmpdata = (float *)tmp.data; + for(int i = 0; i < width*height; i++){ + tmpdata[i] = 0; + } + channels_dy.push_back(tmp); + } + + for(int c = 0; c < channels_base.size(); c++){ + cv::Mat tmp; + tmp.create(height,width,CV_32FC1); + float * tmpdata = (float *)tmp.data; + for(int i = 0; i < width*height; i++){ + tmpdata[i] = 0; + } + channels_errx.push_back(tmp); + } + + for(int c = 0; c < channels_base.size(); c++){ + cv::Mat tmp; + tmp.create(height,width,CV_32FC1); + float * tmpdata = (float *)tmp.data; + for(int i = 0; i < width*height; i++){ + tmpdata[i] = 0; + } + channels_erry.push_back(tmp); + } + + //Shared weights + cv::Mat wximg; + wximg.create(height,width,CV_32FC1); + float * wx = (float *)wximg.data; + for(int i = 0; i < width*height; i++){wx[i] = 1;} + + cv::Mat wyimg; + wyimg.create(height,width,CV_32FC1); + float * wy = (float *)wyimg.data; + for(int i = 0; i < width*height; i++){wy[i] = 1;} + + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(width*height); + + boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0, 0, 0); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + for(int it = 0; true ; it++){ + + if(it % 1 == 0){ + for(int c = 0; c < channels_base.size(); c++){ + float * est = (float *)(channels_est[c].data); + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::PointXYZRGBA & p = cloud->points[ind]; + double z = est[ind]; + if(c == 0){ + if(w % 40 == 0 && h % 40 == 0){printf("%i %i -> %f\n",w,h,z);} + if(z > 0){ + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + }else{ + p.x = 0; + p.y = 0; + p.z = 0; + } + } + if(c == 1){p.b = z*256.0;} + if(c == 2){p.g = z*256.0;} + if(c == 3){p.r = z*256.0;} + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + + viewer->removeAllShapes(); + for(int w0 = 0; w0 < width-1; w0++){ + for(int h0 = 0; h0 < height;h0+=120){ + int ind0 = h0*width+w0; + + int w1 = w0+1; + int h1 = h0; + int ind1 = h1*width+w1; + + pcl::PointXYZRGBA & p0 = cloud->points[ind0]; + pcl::PointXYZRGBA & p1 = cloud->points[ind1]; + if(p0.z == 0 || p1.z == 0){continue;} + + float weight = weightFunc(w0,h0,w1,h1,width, channels_est, channels_dx, channels_dy); + + char buf [1024]; + sprintf(buf,"line_%i_%i_%i_%i",w0,h0,w1,h1); + viewer->addLine (p0,p1,(1-weight),weight,0,buf); + } + } + + for(int w0 = 0; w0 < width; w0+=120){ + for(int h0 = 0; h0 < height-1;h0++){ + int ind0 = h0*width+w0; + + int w1 = w0; + int h1 = h0+1; + int ind1 = h1*width+w1; + + pcl::PointXYZRGBA & p0 = cloud->points[ind0]; + pcl::PointXYZRGBA & p1 = cloud->points[ind1]; + if(p0.z == 0 || p1.z == 0){continue;} + + float weight = weightFunc(w0,h0,w1,h1,width, channels_est, channels_dx, channels_dy); + + char buf [1024]; + sprintf(buf,"line_%i_%i_%i_%i",w0,h0,w1,h1); + viewer->addLine (p0,p1,(1-weight),weight,0,buf); + } + } + viewer->spin(); + + //float pred(float targetW, float targetH, int sourceW, int sourceH, int width, float * est, float * dx, float * dy) + + + } + + //Update errors + for(int c = 0; c < channels_base.size(); c++){ + float * estdata = (float *)(channels_est[c].data); + + float * dxdata = (float *)(channels_dx[c].data); + float * dydata = (float *)(channels_dy[c].data); + + float * errxdata = (float *)(channels_errx[c].data); + float * errydata = (float *)(channels_erry[c].data); + for(int w = 0; w < width-1; w++){ + for(int h = 0; h < height;h++){ + int i = h*width+w; + int i2 = i+1; + float z0 = estdata[i]; + float z1 = estdata[i2]; + float slope0 = dxdata[i]; + float slope1 = dxdata[i2]; + + float e0 = (z0+slope0)-z1; + float e1 = (z1-slope1)-z0; + + errxdata[i] = fabs(e0)+fabs(e1); + } + } + + for(int w = 0; w < width; w++){ + for(int h = 0; h < height-1;h++){ + int i = h*width+w; + int i2 = i+width; + float z0 = estdata[i]; + float z1 = estdata[i2]; + float slope0 = dydata[i]; + float slope1 = dydata[i2]; + + float e0 = (z0+slope0)-z1; + float e1 = (z1-slope1)-z0; + + errydata[i] = fabs(e0)+fabs(e1); + } + } + } + + //Update weights + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int i = h*width+w; + float weight = 1; + for(int c = 0; c < channels_base.size(); c++){ + float e = ((float *)(channels_errx[c].data))[i]; + if(c == 0){weight *= exp(-0.5*e*e/0.1);} + if(c == 1){weight *= exp(-0.5*e*e/0.005);} + if(c == 2){weight *= exp(-0.5*e*e/0.005);} + if(c == 3){weight *= exp(-0.5*e*e/0.005);} + } + wx[i] = weight; + } + } + + //Update weights + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int i = h*width+w; + float weight = 1; + for(int c = 0; c < channels_base.size(); c++){ + float e = ((float *)(channels_erry[c].data))[i]; + if(c == 0){weight *= exp(-0.5*e*e/0.1);} + if(c == 1){weight *= exp(-0.5*e*e/0.005);} + if(c == 2){weight *= exp(-0.5*e*e/0.005);} + if(c == 3){weight *= exp(-0.5*e*e/0.005);} + } + wy[i] = weight; + } + } + +// cv::namedWindow( "wx", cv::WINDOW_AUTOSIZE ); cv::imshow( "wx", wximg); +// cv::namedWindow( "wy", cv::WINDOW_AUTOSIZE ); cv::imshow( "wy", wyimg); +// cv::waitKey(0); + + //Update points + double wbase = 0.1; + for(int c = 0; c < channels_base.size(); c++){ + float * base = (float *)(channels_base[c].data); + float * est = (float *)(channels_est[c].data); + float * dx = (float *)(channels_dx[c].data); + float * dy = (float *)(channels_dy[c].data); + for(int w = 1; w < width-1; w++){ + for(int h = 1; h < height-1;h++){ + int i = h*width+w; + + float wtmp = wbase * float(base[i] > 0); + if(wtmp == 0){continue;} + float w0 = wx[i-1] * float(base[i-1] > 0); + float w1 = wx[i] * float(base[i+1] > 0); + float w2 = wy[i-1] * float(base[i-width] > 0); + float w3 = wy[i] * float(base[i+width] > 0); + + float z = base[i]; + float pred0 = est[i-1] +dx[i-1]; + float pred1 = est[i+1] -dx[i+1]; + float pred2 = est[i-width] +dy[i-width]; + float pred3 = est[i+width] -dy[i+width]; + + double sumz = wtmp*z+w0*pred0+w1*pred1+w2*pred2+w3*pred3; + double sumw = wtmp+w0+w1+w2+w3; + + if(false && w % 40 == 0 && h % 40 == 0){ + printf("%4.4i %4.4i %4.4i -> M(%4.4f %4.4f) ",c,w,h,z,wbase); + printf("est: %4.4f %4.4f %4.4f ", est[i-1],est[i],est[i+1]); + printf("dx: %4.4f %4.4f %4.4f ", dx[i-1], dx[i], dx[i+1]); + printf("wx: %4.4f %4.4f", wx[i-1], wx[i]); + printf("\n"); + } + + //if(w % 40 == 0 && h % 40 == 0){printf("%i %i %i -> M(%4.4f %4.4f) L(%4.4f %4.4f) R(%4.4f %4.4f) U(%4.4f %4.4f) D(%4.4f %4.4f)\n",c,w,h,z,wbase,pred0,w0,pred1,w1,pred2,w2,pred3,w3);} + if(sumw > 0){ + est[i] = sumz/sumw; + }else{ + //est[i] = 0; + } + + } + } +// exit(0); + } + + //Update slopes + + + //} + +// cv::namedWindow( "wx", cv::WINDOW_AUTOSIZE ); cv::imshow( "wx", wximg); +// cv::namedWindow( "wy", cv::WINDOW_AUTOSIZE ); cv::imshow( "wy", wyimg); +// cv::waitKey(0); + } + + // cv::namedWindow( "est", cv::WINDOW_AUTOSIZE ); cv::imshow( "est", channels_est[c] ); + // cv::namedWindow( "dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "dx", channels_dx[c] ); + // cv::namedWindow( "dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "dy", channels_dy[c] ); + // cv::namedWindow( "errx", cv::WINDOW_AUTOSIZE ); cv::imshow( "errx", channels_errx[c]); + // cv::namedWindow( "erry", cv::WINDOW_AUTOSIZE ); cv::imshow( "erry", channels_erry[c]); + // cv::waitKey(0); +exit(0); + //Shared weights + +//Diffs + cv::Mat diffzimg; + diffzimg.create(height,width,CV_32FC2); + float * diffz = (float *)diffzimg.data; + + cv::Mat diffrimg; + diffrimg.create(height,width,CV_32FC2); + float * diffr = (float *)diffrimg.data; + + cv::Mat diffgimg; + diffgimg.create(height,width,CV_32FC2); + float * diffg = (float *)diffgimg.data; + + cv::Mat diffbimg; + diffbimg.create(height,width,CV_32FC2); + float * diffb = (float *)diffbimg.data; + + cv::Mat colimg_est; colimg_est.create(height,width,CV_32FC3); @@ -191,9 +558,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt z_est.create(height,width,CV_32FC3); float * zest = (float *)z_est.data; - cv::Mat z_base; - z_base.create(height,width,CV_32FC3); - float * zbase = (float *)z_base.data; + cv::Mat nimg; nimg.create(height,width,CV_32FC3); @@ -204,19 +569,10 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt dz_est.create(height,width,CV_32FC2); float * dzest = (float *)dz_est.data; - for(int i = 0; i < width*height; i++){ - zbase[i] = zest[i] = idepth*double(depthdata[i]); - } - double w0 = 0.1; - for(int w = 90; w < 1110; w++){ - for(int h = 0; h < height;h++){ - int i = h*width+w; - if( h == 100){ - printf("%i: base: %5.5f %5.5f\n",w,zbase[i],zest[i]); - } - } + for(int i = 0; i < width*height; i++){ + zbase[i] = zest[i] = idepth*double(depthdata[i]); } for(int i = 0; i < 3*width*height; i++){ @@ -228,30 +584,10 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt int i = h*width+w; dzest[2*i+0] = 0; dzest[2*i+1] = 0; -// double z = zest[i]; -// if(w < width-1){ -// dzest[2*i+0] = z-zest[i+1]; -// }else{ -// dzest[2*i+0] = dzest[2*i-2]; -// } - -// if(h < height-1){ -// dzest[2*i+1] = z-zest[i+width]; -// }else{ -// dzest[2*i+1] = dzest[2*(i-width)+1]; -// } } } - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - cloud->width = width; - cloud->height = height; - cloud->points.resize(width*height); - boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0, 0, 0); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); for(int it = 0; true; it++){ printf("it: %i\n",it); diff --git a/quasimodo/quasimodo_msgs/srv/quasimodo_metaroom_segmentation.srv.txt b/quasimodo/quasimodo_msgs/srv/quasimodo_metaroom_segmentation.srv.txt new file mode 100644 index 00000000..fc2665e3 --- /dev/null +++ b/quasimodo/quasimodo_msgs/srv/quasimodo_metaroom_segmentation.srv.txt @@ -0,0 +1,4 @@ +string background +string foreground +--- +sensor_msgs/Image[] segmentation From e6fdfd8ea2b8ce08ded11b7d7ac46f6a71384083 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 10 Aug 2016 23:06:26 +0200 Subject: [PATCH 057/255] ... --- .../quasimodo_models/src/core/RGBDFrame.cpp | 2213 +++++++++-------- 1 file changed, 1189 insertions(+), 1024 deletions(-) diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 95954416..7492113e 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -45,10 +45,10 @@ namespace reglib { unsigned long RGBDFrame_id_counter; RGBDFrame::RGBDFrame(){ - id = RGBDFrame_id_counter++; - capturetime = 0; - pose = Eigen::Matrix4d::Identity(); - keyval = ""; + id = RGBDFrame_id_counter++; + capturetime = 0; + pose = Eigen::Matrix4d::Identity(); + keyval = ""; } bool updated = true; @@ -58,487 +58,652 @@ void on_trackbar( int, void* ){updated = true;} //} float pred(float targetW, float targetH, int sourceW, int sourceH, int width, float * est, float * dx, float * dy){ - int ind = sourceH * width + sourceW; - return est[ind]+(targetW-float(sourceW))*dx[ind]+(targetH-float(sourceH))*dy[ind]; + int ind = sourceH * width + sourceW; + return est[ind]+(targetW-float(sourceW))*dx[ind]+(targetH-float(sourceH))*dy[ind]; } float weightFunc(int w0, int h0, int w1, int h1, int width, std::vector & est, std::vector & dx, std::vector & dy){ - float weight = 1; - for(int c = 0; c < est.size(); c++){ - float error = ((float*)(est[c].data))[h0*width+w0] - pred(w0,h0,w1,h1,width, (float*)(est[c].data), (float*)(dx[c].data), (float*)(dy[c].data)); - weight *= exp(-0.5*error*error/(0.1*0.1)); - } - return weight; + float weight = 1; + for(int c = 0; c < est.size(); c++){ + float error = ((float*)(est[c].data))[h0*width+w0] - pred(w0,h0,w1,h1,width, (float*)(est[c].data), (float*)(dx[c].data), (float*)(dy[c].data)); + weight *= exp(-0.5*error*error/(0.1*0.1)); + } + return weight; +} + + +float weightFunc(int w0, int h0, int w1, int h1, int width, std::vector & est, std::vector & dx, std::vector & dy){ + float weight = 1; + for(int c = 0; c < est.size(); c++){ + float error = est[c][h0*width+w0] - pred(w0,h0,w1,h1,width, est[c], dx[c], dy[c]); + weight *= exp(-0.5*error*error/(0.1*0.1)); + } + return weight; } RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals){ - keyval = ""; + keyval = ""; sweepid = -1; - id = RGBDFrame_id_counter++; - camera = camera_; + id = RGBDFrame_id_counter++; + camera = camera_; rgb = rgb_.clone(); depth = depth_.clone(); - capturetime = capturetime_; - pose = pose_; + capturetime = capturetime_; + pose = pose_; - IplImage iplimg = rgb_; - IplImage* img = &iplimg; + IplImage iplimg = rgb_; + IplImage* img = &iplimg; - //printf("%s LINE:%i\n",__FILE__,__LINE__); + //printf("%s LINE:%i\n",__FILE__,__LINE__); int width = img->width; int height = img->height; int sz = height*width; - const double idepth = camera->idepth_scale; - const double cx = camera->cx; - const double cy = camera->cy; - const double ifx = 1.0/camera->fx; - const double ify = 1.0/camera->fy; + const double idepth = camera->idepth_scale; + const double cx = camera->cx; + const double cy = camera->cy; + const double ifx = 1.0/camera->fx; + const double ify = 1.0/camera->fy; - // -// printf("camera: cx %5.5f cy %5.5f",cx,cy); -// printf(": fx %5.5f fy %5.5f",camera->fx,camera->fy); -// printf(": ifx %5.5f ify %5.5f\n",ifx,ify); + // + // printf("camera: cx %5.5f cy %5.5f",cx,cy); + // printf(": fx %5.5f fy %5.5f",camera->fx,camera->fy); + // printf(": ifx %5.5f ify %5.5f\n",ifx,ify); - connections.resize(1); - connections[0].resize(1); - connections[0][0] = 0; + connections.resize(1); + connections[0].resize(1); + connections[0][0] = 0; - intersections.resize(1); - intersections[0].resize(1); - intersections[0][0] = 0; + intersections.resize(1); + intersections[0].resize(1); + intersections[0][0] = 0; nr_labels = 1; labels = new int[width*height]; for(int i = 0; i < width*height; i++){labels[i] = 0;} - //printf("%s LINE:%i\n",__FILE__,__LINE__); - unsigned short * depthdata = (unsigned short *)depth.data; - unsigned char * rgbdata = (unsigned char *)rgb.data; + //printf("%s LINE:%i\n",__FILE__,__LINE__); + unsigned short * depthdata = (unsigned short *)depth.data; + unsigned char * rgbdata = (unsigned char *)rgb.data; - depthedges.create(height,width,CV_8UC1); - unsigned char * depthedgesdata = (unsigned char *)depthedges.data; - for(int i = 0; i < width*height; i++){ - depthedgesdata[i] = 0; - } -/* - double t = 0.01; + depthedges.create(height,width,CV_8UC1); + unsigned char * depthedgesdata = (unsigned char *)depthedges.data; + for(int i = 0; i < width*height; i++){ + depthedgesdata[i] = 0; + } + /* + double t = 0.01; for(int w = 0; w < width; w++){ for(int h = 0; h < height;h++){ - int ind = h*width+w; - depthedgesdata[ind] = 0; - double z = idepth*double(depthdata[ind]); - if(w > 0){ - double z2 = idepth*double(depthdata[ind-1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } - - if(w < width-1){ - double z2 = idepth*double(depthdata[ind+1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } - - if(h > 0){ - double z2 = idepth*double(depthdata[ind-width]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } - - if(h < height-1){ - double z2 = idepth*double(depthdata[ind+width]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } - - if(h > 0 && w > 0){ - double z2 = idepth*double(depthdata[ind-width-1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } - - if(w > 0 && h < height-1){ - double z2 = idepth*double(depthdata[ind+width-1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } - - if(h > 0 && w < width-1){ - double z2 = idepth*double(depthdata[ind-width+1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } - - if(h < height-1 && w < width-1){ - double z2 = idepth*double(depthdata[ind+width+1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } - } - } + int ind = h*width+w; + depthedgesdata[ind] = 0; + double z = idepth*double(depthdata[ind]); + if(w > 0){ + double z2 = idepth*double(depthdata[ind-1]); + double info = 1.0/(z*z+z2*z2); + double diff = fabs(z2-z)*info; + if(diff > t){depthedgesdata[ind] = 255;} + } + + if(w < width-1){ + double z2 = idepth*double(depthdata[ind+1]); + double info = 1.0/(z*z+z2*z2); + double diff = fabs(z2-z)*info; + if(diff > t){depthedgesdata[ind] = 255;} + } + + if(h > 0){ + double z2 = idepth*double(depthdata[ind-width]); + double info = 1.0/(z*z+z2*z2); + double diff = fabs(z2-z)*info; + if(diff > t){depthedgesdata[ind] = 255;} + } + + if(h < height-1){ + double z2 = idepth*double(depthdata[ind+width]); + double info = 1.0/(z*z+z2*z2); + double diff = fabs(z2-z)*info; + if(diff > t){depthedgesdata[ind] = 255;} + } + + if(h > 0 && w > 0){ + double z2 = idepth*double(depthdata[ind-width-1]); + double info = 1.0/(z*z+z2*z2); + double diff = fabs(z2-z)*info; + if(diff > t){depthedgesdata[ind] = 255;} + } + + if(w > 0 && h < height-1){ + double z2 = idepth*double(depthdata[ind+width-1]); + double info = 1.0/(z*z+z2*z2); + double diff = fabs(z2-z)*info; + if(diff > t){depthedgesdata[ind] = 255;} + } + + if(h > 0 && w < width-1){ + double z2 = idepth*double(depthdata[ind-width+1]); + double info = 1.0/(z*z+z2*z2); + double diff = fabs(z2-z)*info; + if(diff > t){depthedgesdata[ind] = 255;} + } + + if(h < height-1 && w < width-1){ + double z2 = idepth*double(depthdata[ind+width+1]); + double info = 1.0/(z*z+z2*z2); + double diff = fabs(z2-z)*info; + if(diff > t){depthedgesdata[ind] = 255;} + } + } + } */ - if(false){ - //Per channel - //Base - //est - //dx - //dy - //errX - //errY - std::vector channels_base; - std::vector channels_noise; - std::vector channels_est; - std::vector channels_dx; - std::vector channels_dy; - std::vector channels_errx; - std::vector channels_erry; - - cv::Mat z_base; - z_base.create(height,width,CV_32FC1); - float * zbase = (float *)z_base.data; - - for(int i = 0; i < width*height; i++){ - zbase[i] = idepth*double(depthdata[i]); - } - - channels_base.push_back(z_base); - for(int c = 0; c < 3; c++){ - cv::Mat current_base; - current_base.create(height,width,CV_32FC1); - float * currentbase = (float *)current_base.data; - - for(int i = 0; i < width*height; i++){ - currentbase[i] = float(rgbdata[3*i+c])/256.0; - } - channels_base.push_back(current_base); - } - - for(int c = 0; c < channels_base.size(); c++){ - float * data = (float *)(channels_base[c].data); - cv::Mat tmp; - tmp.create(height,width,CV_32FC1); - float * tmpdata = (float *)tmp.data; - for(int i = 0; i < width*height; i++){ - tmpdata[i] = data[i]; - } - channels_est.push_back(tmp); - } - - for(int c = 0; c < channels_base.size(); c++){ - cv::Mat tmp; - tmp.create(height,width,CV_32FC1); - float * tmpdata = (float *)tmp.data; - for(int i = 0; i < width*height; i++){ - tmpdata[i] = 0; - } - channels_dx.push_back(tmp); - } - - for(int c = 0; c < channels_base.size(); c++){ - cv::Mat tmp; - tmp.create(height,width,CV_32FC1); - float * tmpdata = (float *)tmp.data; - for(int i = 0; i < width*height; i++){ - tmpdata[i] = 0; - } - channels_dy.push_back(tmp); - } - - for(int c = 0; c < channels_base.size(); c++){ - cv::Mat tmp; - tmp.create(height,width,CV_32FC1); - float * tmpdata = (float *)tmp.data; - for(int i = 0; i < width*height; i++){ - tmpdata[i] = 0; - } - channels_errx.push_back(tmp); - } - - for(int c = 0; c < channels_base.size(); c++){ - cv::Mat tmp; - tmp.create(height,width,CV_32FC1); - float * tmpdata = (float *)tmp.data; - for(int i = 0; i < width*height; i++){ - tmpdata[i] = 0; - } - channels_erry.push_back(tmp); - } - - //Shared weights - cv::Mat wximg; - wximg.create(height,width,CV_32FC1); - float * wx = (float *)wximg.data; - for(int i = 0; i < width*height; i++){wx[i] = 1;} - - cv::Mat wyimg; - wyimg.create(height,width,CV_32FC1); - float * wy = (float *)wyimg.data; - for(int i = 0; i < width*height; i++){wy[i] = 1;} - - - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - cloud->width = width; - cloud->height = height; - cloud->points.resize(width*height); - - boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0, 0, 0); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); - - for(int it = 0; true ; it++){ - - if(it % 1 == 0){ - for(int c = 0; c < channels_base.size(); c++){ - float * est = (float *)(channels_est[c].data); - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBA & p = cloud->points[ind]; - double z = est[ind]; - if(c == 0){ - if(w % 40 == 0 && h % 40 == 0){printf("%i %i -> %f\n",w,h,z);} - if(z > 0){ - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - }else{ - p.x = 0; - p.y = 0; - p.z = 0; - } - } - if(c == 1){p.b = z*256.0;} - if(c == 2){p.g = z*256.0;} - if(c == 3){p.r = z*256.0;} - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - - viewer->removeAllShapes(); - for(int w0 = 0; w0 < width-1; w0++){ - for(int h0 = 0; h0 < height;h0+=120){ - int ind0 = h0*width+w0; - - int w1 = w0+1; - int h1 = h0; - int ind1 = h1*width+w1; - - pcl::PointXYZRGBA & p0 = cloud->points[ind0]; - pcl::PointXYZRGBA & p1 = cloud->points[ind1]; - if(p0.z == 0 || p1.z == 0){continue;} - - float weight = weightFunc(w0,h0,w1,h1,width, channels_est, channels_dx, channels_dy); - - char buf [1024]; - sprintf(buf,"line_%i_%i_%i_%i",w0,h0,w1,h1); - viewer->addLine (p0,p1,(1-weight),weight,0,buf); - } - } - - for(int w0 = 0; w0 < width; w0+=120){ - for(int h0 = 0; h0 < height-1;h0++){ - int ind0 = h0*width+w0; - - int w1 = w0; - int h1 = h0+1; - int ind1 = h1*width+w1; - - pcl::PointXYZRGBA & p0 = cloud->points[ind0]; - pcl::PointXYZRGBA & p1 = cloud->points[ind1]; - if(p0.z == 0 || p1.z == 0){continue;} - - float weight = weightFunc(w0,h0,w1,h1,width, channels_est, channels_dx, channels_dy); - - char buf [1024]; - sprintf(buf,"line_%i_%i_%i_%i",w0,h0,w1,h1); - viewer->addLine (p0,p1,(1-weight),weight,0,buf); - } - } - viewer->spin(); - - //float pred(float targetW, float targetH, int sourceW, int sourceH, int width, float * est, float * dx, float * dy) - - - } - - //Update errors - for(int c = 0; c < channels_base.size(); c++){ - float * estdata = (float *)(channels_est[c].data); - - float * dxdata = (float *)(channels_dx[c].data); - float * dydata = (float *)(channels_dy[c].data); - - float * errxdata = (float *)(channels_errx[c].data); - float * errydata = (float *)(channels_erry[c].data); - for(int w = 0; w < width-1; w++){ - for(int h = 0; h < height;h++){ - int i = h*width+w; - int i2 = i+1; - float z0 = estdata[i]; - float z1 = estdata[i2]; - float slope0 = dxdata[i]; - float slope1 = dxdata[i2]; - - float e0 = (z0+slope0)-z1; - float e1 = (z1-slope1)-z0; - - errxdata[i] = fabs(e0)+fabs(e1); - } - } - - for(int w = 0; w < width; w++){ - for(int h = 0; h < height-1;h++){ - int i = h*width+w; - int i2 = i+width; - float z0 = estdata[i]; - float z1 = estdata[i2]; - float slope0 = dydata[i]; - float slope1 = dydata[i2]; - - float e0 = (z0+slope0)-z1; - float e1 = (z1-slope1)-z0; - - errydata[i] = fabs(e0)+fabs(e1); - } - } - } - - //Update weights - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int i = h*width+w; - float weight = 1; - for(int c = 0; c < channels_base.size(); c++){ - float e = ((float *)(channels_errx[c].data))[i]; - if(c == 0){weight *= exp(-0.5*e*e/0.1);} - if(c == 1){weight *= exp(-0.5*e*e/0.005);} - if(c == 2){weight *= exp(-0.5*e*e/0.005);} - if(c == 3){weight *= exp(-0.5*e*e/0.005);} - } - wx[i] = weight; - } - } - - //Update weights - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int i = h*width+w; - float weight = 1; - for(int c = 0; c < channels_base.size(); c++){ - float e = ((float *)(channels_erry[c].data))[i]; - if(c == 0){weight *= exp(-0.5*e*e/0.1);} - if(c == 1){weight *= exp(-0.5*e*e/0.005);} - if(c == 2){weight *= exp(-0.5*e*e/0.005);} - if(c == 3){weight *= exp(-0.5*e*e/0.005);} - } - wy[i] = weight; - } - } + // cv::Mat src = rgb.clone(); -// cv::namedWindow( "wx", cv::WINDOW_AUTOSIZE ); cv::imshow( "wx", wximg); + // for(int i = 0; i < 20; i++){ + // cv::Mat dst; + // cv::adaptiveBilateralFilter(src, dst, cv::Size(7,7), 30); + + // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); + // cv::namedWindow( "bilat", cv::WINDOW_AUTOSIZE ); cv::imshow( "bilat", dst); + // cv::waitKey(0); + // src = dst.clone(); + // } + + if(true){ + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(width*height); + + boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0, 0, 0); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + float * zbase = new float[width*height]; + float * rbase = new float[width*height]; + float * gbase = new float[width*height]; + float * bbase = new float[width*height]; + + float * zest = new float[width*height]; + float * rest = new float[width*height]; + float * gest = new float[width*height]; + float * best = new float[width*height]; + + + float * znext = new float[width*height]; + float * rnext = new float[width*height]; + float * gnext = new float[width*height]; + float * bnext = new float[width*height]; + + for(int i = 0; i < width*height; i++){ + zbase[i] = zest[i] = idepth*double(depthdata[i]); + bbase[i] = best[i] = float(rgbdata[3*i+0])/256.0; + gbase[i] = gest[i] = float(rgbdata[3*i+1])/256.0; + rbase[i] = rest[i] = float(rgbdata[3*i+2])/256.0; + } + + for(int it = 0; true; it++){ + if(it % 1 == 0){ + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::PointXYZRGBA & p = cloud->points[ind]; + double z = zest[ind]; + if(z > 0){ + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + }else{ + p.x = 0; + p.y = 0; + p.z = 0; + } + p.b = best[ind]*256.0; + p.g = gest[ind]*256.0; + p.r = rest[ind]*256.0; + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } + + + for(int w = 1; w < width-1; w++){ + for(int h = 1; h < height-1;h++){ + int iM = h*width+w; + int iL = iM-1; + int iR = iM+1; + int iU = iM-width; + int iD = iM+width; + + float zMbase = zbase[iM]; + float zM = zest[iM]; + float zL = zest[iL]; + float zR = zest[iR]; + float zU = zest[iU]; + float zD = zest[iD]; + + float rMbase = rbase[iM]; + float rM = rest[iM]; + float rL = rest[iL]; + float rR = rest[iR]; + float rU = rest[iU]; + float rD = rest[iD]; + + float gMbase = gbase[iM]; + float gM = gest[iM]; + float gL = gest[iL]; + float gR = gest[iR]; + float gU = gest[iU]; + float gD = gest[iD]; + + float bMbase = bbase[iM]; + float bM = best[iM]; + float bL = best[iL]; + float bR = best[iR]; + float bU = best[iU]; + float bD = best[iD]; + + + float dzL = zM-zL; + float dzR = zM-zR; + float dzU = zM-zU; + float dzD = zM-zD; + + } + } + } + exit(0); + } + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(width*height); + + boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0, 0, 0); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + if(false){ + // cv::Mat bilat; + // cv::adaptiveBilateralFilter(rgb, bilat, cv::Size(21,21), 30); + + // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); + // cv::namedWindow( "bilat", cv::WINDOW_AUTOSIZE ); cv::imshow( "bilat", bilat); + // cv::waitKey(0); + //Per channel + //Base + //est + //dx + //dy + //errX + //errY + std::vector channels_base; + std::vector channels_noise; + std::vector channels_est; + std::vector channels_dx; + std::vector channels_dy; + std::vector channels_errx; + std::vector channels_erry; + + cv::Mat z_base; + z_base.create(height,width,CV_32FC1); + float * zbase = (float *)z_base.data; + + for(int i = 0; i < width*height; i++){ + zbase[i] = idepth*double(depthdata[i]); + } + + channels_base.push_back(z_base); + for(int c = 0; c < 3; c++){ + cv::Mat current_base; + current_base.create(height,width,CV_32FC1); + float * currentbase = (float *)current_base.data; + + for(int i = 0; i < width*height; i++){ + currentbase[i] = float(rgbdata[3*i+c])/256.0; + } + channels_base.push_back(current_base); + } + + for(int c = 0; c < channels_base.size(); c++){ + float * data = (float *)(channels_base[c].data); + cv::Mat tmp; + tmp.create(height,width,CV_32FC1); + float * tmpdata = (float *)tmp.data; + for(int i = 0; i < width*height; i++){ + tmpdata[i] = data[i]; + } + channels_est.push_back(tmp); + } + + for(int c = 0; c < channels_base.size(); c++){ + cv::Mat tmp; + tmp.create(height,width,CV_32FC1); + float * tmpdata = (float *)tmp.data; + for(int i = 0; i < width*height; i++){ + tmpdata[i] = 0; + } + channels_dx.push_back(tmp); + } + + for(int c = 0; c < channels_base.size(); c++){ + cv::Mat tmp; + tmp.create(height,width,CV_32FC1); + float * tmpdata = (float *)tmp.data; + for(int i = 0; i < width*height; i++){ + tmpdata[i] = 0; + } + channels_dy.push_back(tmp); + } + + for(int c = 0; c < channels_base.size(); c++){ + cv::Mat tmp; + tmp.create(height,width,CV_32FC1); + float * tmpdata = (float *)tmp.data; + for(int i = 0; i < width*height; i++){ + tmpdata[i] = 0; + } + channels_errx.push_back(tmp); + } + + for(int c = 0; c < channels_base.size(); c++){ + cv::Mat tmp; + tmp.create(height,width,CV_32FC1); + float * tmpdata = (float *)tmp.data; + for(int i = 0; i < width*height; i++){ + tmpdata[i] = 0; + } + channels_erry.push_back(tmp); + } + + //Shared weights + cv::Mat wximg; + wximg.create(height,width,CV_32FC1); + float * wx = (float *)wximg.data; + for(int i = 0; i < width*height; i++){wx[i] = 1;} + + cv::Mat wyimg; + wyimg.create(height,width,CV_32FC1); + float * wy = (float *)wyimg.data; + for(int i = 0; i < width*height; i++){wy[i] = 1;} + + + + + for(int it = 0; true ; it++){ + + if(it % 1 == 0){ + for(int c = 0; c < channels_base.size(); c++){ + float * est = (float *)(channels_est[c].data); + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::PointXYZRGBA & p = cloud->points[ind]; + double z = est[ind]; + if(c == 0){ + if(z > 0){ + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + }else{ + p.x = 0; + p.y = 0; + p.z = 0; + } + } + if(c == 1){p.b = z*256.0;} + if(c == 2){p.g = z*256.0;} + if(c == 3){p.r = z*256.0;} + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + + viewer->removeAllShapes(); + for(int w0 = 0; w0 < width-1; w0++){ + for(int h0 = 0; h0 < height;h0+=4){ + int ind0 = h0*width+w0; + + int w1 = w0+1; + int h1 = h0; + int ind1 = h1*width+w1; + + pcl::PointXYZRGBA & p0 = cloud->points[ind0]; + pcl::PointXYZRGBA & p1 = cloud->points[ind1]; + if(p0.z == 0 || p1.z == 0){continue;} + + float weight = weightFunc(w0,h0,w1,h1,width, channels_est, channels_dx, channels_dy); + if(weight > 0.5){continue;} + char buf [1024]; + sprintf(buf,"line_%i_%i_%i_%i",w0,h0,w1,h1); + viewer->addLine (p0,p1,(1-weight),weight,0,buf); + } + } + + for(int w0 = 0; w0 < width; w0+=4){ + for(int h0 = 0; h0 < height-1;h0++){ + int ind0 = h0*width+w0; + + int w1 = w0; + int h1 = h0+1; + int ind1 = h1*width+w1; + + pcl::PointXYZRGBA & p0 = cloud->points[ind0]; + pcl::PointXYZRGBA & p1 = cloud->points[ind1]; + if(p0.z == 0 || p1.z == 0){continue;} + + float weight = weightFunc(w0,h0,w1,h1,width, channels_est, channels_dx, channels_dy); + + if(weight > 0.5){continue;} + char buf [1024]; + sprintf(buf,"line_%i_%i_%i_%i",w0,h0,w1,h1); + viewer->addLine (p0,p1,(1-weight),weight,0,buf); + } + } + viewer->spin(); + + //float pred(float targetW, float targetH, int sourceW, int sourceH, int width, float * est, float * dx, float * dy) + } + + // float mulbase = 0.1; + // for(int w = 1; w < width-1; w++){ + // for(int h = 1; h < height-1;h++){ + // int i = h*width+w; + // float weightBase = mulbase*((float*)(channels_base[0].data))[i] > 0; + // float weightL = weightFunc(w,h,w-1,h ,width, channels_est, channels_dx, channels_dy); + // float weightR = weightFunc(w,h,w+1,h ,width, channels_est, channels_dx, channels_dy); + // float weightU = weightFunc(w,h,w ,h-1,width, channels_est, channels_dx, channels_dy); + // float weightD = weightFunc(w,h,w ,h+1,width, channels_est, channels_dx, channels_dy); + // float sumW = weightBase+weightL+weightR+weightU+weightD; + // if(sumW == 0){continue;} + // for(int c = 0; c < channels_base.size(); c++){ + // float base = ((float*)(channels_base[c].data))[i]; + // float L = pred(w,h,w-1,h,width, (float*)(channels_est[c].data), (float*)(channels_dx[c].data), (float*)(channels_dy[c].data)); + // float R = pred(w,h,w+1,h,width, (float*)(channels_est[c].data), (float*)(channels_dx[c].data), (float*)(channels_dy[c].data)); + // float U = pred(w,h,w,h-1,width, (float*)(channels_est[c].data), (float*)(channels_dx[c].data), (float*)(channels_dy[c].data)); + // float D = pred(w,h,w,h+1,width, (float*)(channels_est[c].data), (float*)(channels_dx[c].data), (float*)(channels_dy[c].data)); + // float estimate = (weightBase*base+weightL*L+weightR*R+weightU*U+weightL*L+weightD*D)/sumW; + // ((float*)(channels_est[c].data))[i] = estimate; + // } + // } + // } + } + //} + + /* + //Update errors + for(int c = 0; c < channels_base.size(); c++){ + float * estdata = (float *)(channels_est[c].data); + + float * dxdata = (float *)(channels_dx[c].data); + float * dydata = (float *)(channels_dy[c].data); + + float * errxdata = (float *)(channels_errx[c].data); + float * errydata = (float *)(channels_erry[c].data); + for(int w = 0; w < width-1; w++){ + for(int h = 0; h < height;h++){ + int i = h*width+w; + int i2 = i+1;// cv::namedWindow( "wx", cv::WINDOW_AUTOSIZE ); cv::imshow( "wx", wximg); // cv::namedWindow( "wy", cv::WINDOW_AUTOSIZE ); cv::imshow( "wy", wyimg); // cv::waitKey(0); + float z0 = estdata[i]; + float z1 = estdata[i2]; + float slope0 = dxdata[i]; + float slope1 = dxdata[i2]; + + float e0 = (z0+slope0)-z1; + float e1 = (z1-slope1)-z0; + + errxdata[i] = fabs(e0)+fabs(e1); + } + } + + for(int w = 0; w < width; w++){ + for(int h = 0; h < height-1;h++){ + int i = h*width+w; + int i2 = i+width; + float z0 = estdata[i]; + float z1 = estdata[i2]; + float slope0 = dydata[i]; + float slope1 = dydata[i2]; + + float e0 = (z0+slope0)-z1; + float e1 = (z1-slope1)-z0; + + errydata[i] = fabs(e0)+fabs(e1); + } + } + } - //Update points - double wbase = 0.1; - for(int c = 0; c < channels_base.size(); c++){ - float * base = (float *)(channels_base[c].data); - float * est = (float *)(channels_est[c].data); - float * dx = (float *)(channels_dx[c].data); - float * dy = (float *)(channels_dy[c].data); - for(int w = 1; w < width-1; w++){ - for(int h = 1; h < height-1;h++){ - int i = h*width+w; - - float wtmp = wbase * float(base[i] > 0); - if(wtmp == 0){continue;} - float w0 = wx[i-1] * float(base[i-1] > 0); - float w1 = wx[i] * float(base[i+1] > 0); - float w2 = wy[i-1] * float(base[i-width] > 0); - float w3 = wy[i] * float(base[i+width] > 0); - - float z = base[i]; - float pred0 = est[i-1] +dx[i-1]; - float pred1 = est[i+1] -dx[i+1]; - float pred2 = est[i-width] +dy[i-width]; - float pred3 = est[i+width] -dy[i+width]; - - double sumz = wtmp*z+w0*pred0+w1*pred1+w2*pred2+w3*pred3; - double sumw = wtmp+w0+w1+w2+w3; - - if(false && w % 40 == 0 && h % 40 == 0){ - printf("%4.4i %4.4i %4.4i -> M(%4.4f %4.4f) ",c,w,h,z,wbase); - printf("est: %4.4f %4.4f %4.4f ", est[i-1],est[i],est[i+1]); - printf("dx: %4.4f %4.4f %4.4f ", dx[i-1], dx[i], dx[i+1]); - printf("wx: %4.4f %4.4f", wx[i-1], wx[i]); - printf("\n"); - } - - //if(w % 40 == 0 && h % 40 == 0){printf("%i %i %i -> M(%4.4f %4.4f) L(%4.4f %4.4f) R(%4.4f %4.4f) U(%4.4f %4.4f) D(%4.4f %4.4f)\n",c,w,h,z,wbase,pred0,w0,pred1,w1,pred2,w2,pred3,w3);} - if(sumw > 0){ - est[i] = sumz/sumw; - }else{ - //est[i] = 0; - } - - } - } -// exit(0); - } - - //Update slopes - - - //} + //Update weights + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int i = h*width+w; + float weight = 1; + for(int c = 0; c < channels_base.size(); c++){ + float e = ((float *)(channels_errx[c].data))[i]; + if(c == 0){weight *= exp(-0.5*e*e/0.1);} + if(c == 1){weight *= exp(-0.5*e*e/0.005);} + if(c == 2){weight *= exp(-0.5*e*e/0.005);} + if(c == 3){weight *= exp(-0.5*e*e/0.005);} + } + wx[i] = weight; + } + } + + //Update weights + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int i = h*width+w; + float weight = 1; + for(int c = 0; c < channels_base.size(); c++){ + float e = ((float *)(channels_erry[c].data))[i]; + if(c == 0){weight *= exp(-0.5*e*e/0.1);} + if(c == 1){weight *= exp(-0.5*e*e/0.005);} + if(c == 2){weight *= exp(-0.5*e*e/0.005);} + if(c == 3){weight *= exp(-0.5*e*e/0.005);} + } + wy[i] = weight; + } + } // cv::namedWindow( "wx", cv::WINDOW_AUTOSIZE ); cv::imshow( "wx", wximg); // cv::namedWindow( "wy", cv::WINDOW_AUTOSIZE ); cv::imshow( "wy", wyimg); // cv::waitKey(0); - } - // cv::namedWindow( "est", cv::WINDOW_AUTOSIZE ); cv::imshow( "est", channels_est[c] ); - // cv::namedWindow( "dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "dx", channels_dx[c] ); - // cv::namedWindow( "dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "dy", channels_dy[c] ); - // cv::namedWindow( "errx", cv::WINDOW_AUTOSIZE ); cv::imshow( "errx", channels_errx[c]); - // cv::namedWindow( "erry", cv::WINDOW_AUTOSIZE ); cv::imshow( "erry", channels_erry[c]); - // cv::waitKey(0); -exit(0); - //Shared weights + //Update points + double wbase = 0.1; + for(int c = 0; c < channels_base.size(); c++){ + float * base = (float *)(channels_base[c].data); + float * est = (float *)(channels_est[c].data); + float * dx = (float *)(channels_dx[c].data); + float * dy = (float *)(channels_dy[c].data); + for(int w = 1; w < width-1; w++){ + for(int h = 1; h < height-1;h++){ + int i = h*width+w; + + float wtmp = wbase * float(base[i] > 0); + if(wtmp == 0){continue;} + float w0 = wx[i-1] * float(base[i-1] > 0); + float w1 = wx[i] * float(base[i+1] > 0); + float w2 = wy[i-1] * float(base[i-width] > 0); + float w3 = wy[i] * float(base[i+width] > 0); + + float z = base[i]; + float pred0 = est[i-1] +dx[i-1]; + float pred1 = est[i+1] -dx[i+1]; + float pred2 = est[i-width] +dy[i-width]; + float pred3 = est[i+width] -dy[i+width]; + + double sumz = wtmp*z+w0*pred0+w1*pred1+w2*pred2+w3*pred3; + double sumw = wtmp+w0+w1+w2+w3; + + if(false && w % 40 == 0 && h % 40 == 0){ + printf("%4.4i %4.4i %4.4i -> M(%4.4f %4.4f) ",c,w,h,z,wbase); + printf("est: %4.4f %4.4f %4.4f ", est[i-1],est[i],est[i+1]); + printf("dx: %4.4f %4.4f %4.4f ", dx[i-1], dx[i], dx[i+1]); + printf("wx: %4.4f %4.4f", wx[i-1], wx[i]); + printf("\n"); + } -//Diffs - cv::Mat diffzimg; - diffzimg.create(height,width,CV_32FC2); - float * diffz = (float *)diffzimg.data; + //if(w % 40 == 0 && h % 40 == 0){printf("%i %i %i -> M(%4.4f %4.4f) L(%4.4f %4.4f) R(%4.4f %4.4f) U(%4.4f %4.4f) D(%4.4f %4.4f)\n",c,w,h,z,wbase,pred0,w0,pred1,w1,pred2,w2,pred3,w3);} + if(sumw > 0){ + est[i] = sumz/sumw; + }else{ + //est[i] = 0; + } + + } + } + */ + // exit(0); + // } + + // //Update slopes + + + // //} - cv::Mat diffrimg; - diffrimg.create(height,width,CV_32FC2); - float * diffr = (float *)diffrimg.data; + //// cv::namedWindow( "wx", cv::WINDOW_AUTOSIZE ); cv::imshow( "wx", wximg); + //// cv::namedWindow( "wy", cv::WINDOW_AUTOSIZE ); cv::imshow( "wy", wyimg); + //// cv::waitKey(0); + // } - cv::Mat diffgimg; - diffgimg.create(height,width,CV_32FC2); - float * diffg = (float *)diffgimg.data; + // cv::namedWindow( "est", cv::WINDOW_AUTOSIZE ); cv::imshow( "est", channels_est[c] ); + // cv::namedWindow( "dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "dx", channels_dx[c] ); + // cv::namedWindow( "dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "dy", channels_dy[c] ); + // cv::namedWindow( "errx", cv::WINDOW_AUTOSIZE ); cv::imshow( "errx", channels_errx[c]); + // cv::namedWindow( "erry", cv::WINDOW_AUTOSIZE ); cv::imshow( "erry", channels_erry[c]); + // cv::waitKey(0); + exit(0); + //Shared weights - cv::Mat diffbimg; - diffbimg.create(height,width,CV_32FC2); - float * diffb = (float *)diffbimg.data; + //Diffs + cv::Mat diffzimg; + diffzimg.create(height,width,CV_32FC2); + float * diffz = (float *)diffzimg.data; + + cv::Mat diffrimg; + diffrimg.create(height,width,CV_32FC2); + float * diffr = (float *)diffrimg.data; + + cv::Mat diffgimg; + diffgimg.create(height,width,CV_32FC2); + float * diffg = (float *)diffgimg.data; + + cv::Mat diffbimg; + diffbimg.create(height,width,CV_32FC2); + float * diffb = (float *)diffbimg.data; @@ -617,24 +782,24 @@ exit(0); viewer->spin(); } -//// Compute normals -// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", colimg_est ); -// cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); cv::imshow( "normals", nimg ); -// cv::namedWindow( "dz", cv::WINDOW_AUTOSIZE ); cv::imshow( "dz", dz_est ); -// cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", z_est); -// cv::waitKey(0); - -// for(int w = 0; w < width; w++){ -// for(int h = 0; h < height;h++){ -// int i = h*width+w; -// double z = zest[i]; -// if(w < width-1){ dzest[2*i+0] = z-zest[i+1];} -// else{ dzest[2*i+0] = dzest[2*i-2];} - -// if(h < height-1){ dzest[2*i+1] = z-zest[i+width];} -// else{ dzest[2*i+1] = dzest[2*(i-width)+1];} -// } -// } + //// Compute normals + // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", colimg_est ); + // cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); cv::imshow( "normals", nimg ); + // cv::namedWindow( "dz", cv::WINDOW_AUTOSIZE ); cv::imshow( "dz", dz_est ); + // cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", z_est); + // cv::waitKey(0); + + // for(int w = 0; w < width; w++){ + // for(int h = 0; h < height;h++){ + // int i = h*width+w; + // double z = zest[i]; + // if(w < width-1){ dzest[2*i+0] = z-zest[i+1];} + // else{ dzest[2*i+0] = dzest[2*i-2];} + + // if(h < height-1){ dzest[2*i+1] = z-zest[i+width];} + // else{ dzest[2*i+1] = dzest[2*(i-width)+1];} + // } + // } double w0 = 0.001; for(int w = 0; w < width; w++){ @@ -661,9 +826,9 @@ exit(0); sumz+=weight*predz; sumw+=weight; -// if(w == 100 && h == 100){ -// printf("left %i: base: %5.5f %5.5f\n",w-1,zbase[i2],zest[i2]); -// } + // if(w == 100 && h == 100){ + // printf("left %i: base: %5.5f %5.5f\n",w-1,zbase[i2],zest[i2]); + // } } if(w < width-1){ @@ -701,249 +866,249 @@ exit(0); } -// double * z_base = new double[width*height]; -// double * col_base = new double[3*width*height]; -// double * z_est = new double[width*height]; -// double * col_est = new double[3*width*height]; -// double * normal_est = new double[3*width*height]; + // double * z_base = new double[width*height]; + // double * col_base = new double[3*width*height]; + // double * z_est = new double[width*height]; + // double * col_est = new double[3*width*height]; + // double * normal_est = new double[3*width*height]; -// for(int i = 0; i < width*height; i++){ -// z_base[i] = z_est[i] = idepth*double(depthdata[ind]); -// col_est[3*i+0] = rgbdata[3*i+0]; -// col_est[3*i+1] = rgbdata[3*i+1]; -// col_est[3*i+2] = rgbdata[3*i+2]; -// } -// delete[] z_base; -// delete[] col_base; -// delete[] z_est; -// delete[] col_est; -// delete[] normal_est; + // for(int i = 0; i < width*height; i++){ + // z_base[i] = z_est[i] = idepth*double(depthdata[ind]); + // col_est[3*i+0] = rgbdata[3*i+0]; + // col_est[3*i+1] = rgbdata[3*i+1]; + // col_est[3*i+2] = rgbdata[3*i+2]; + // } + // delete[] z_base; + // delete[] col_base; + // delete[] z_est; + // delete[] col_est; + // delete[] normal_est; } - //printf("%s LINE:%i\n",__FILE__,__LINE__); - if(compute_normals){ - normals.create(height,width,CV_32FC3); - float * normalsdata = (float *)normals.data; + //printf("%s LINE:%i\n",__FILE__,__LINE__); + if(compute_normals){ + normals.create(height,width,CV_32FC3); + float * normalsdata = (float *)normals.data; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr normals_cloud (new pcl::PointCloud); - cloud->width = width; - cloud->height = height; - cloud->points.resize(width*height); - //cloud->dense = false; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr normals_cloud (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(width*height); + //cloud->dense = false; for(int w = 0; w < width; w++){ for(int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBA & p = cloud->points[ind]; - p.b = rgbdata[3*ind+0]; - p.g = rgbdata[3*ind+1]; - p.r = rgbdata[3*ind+2]; - double z = idepth*double(depthdata[ind]); - if(z > 0){ - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - }else{ - p.x = NAN; - p.y = NAN; - p.z = NAN; - } - } - } - - //printf("%s LINE:%i\n",__FILE__,__LINE__); - pcl::IntegralImageNormalEstimation ne; - ne.setInputCloud(cloud); - - bool tune = false; - unsigned char * combidata; - cv::Mat combined; - - int NormalEstimationMethod = 0; - int MaxDepthChangeFactor = 20; - int NormalSmoothingSize = 7; - int depth_dependent_smoothing = 1; - - ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); - ne.setNormalSmoothingSize(NormalSmoothingSize); - ne.setDepthDependentSmoothing (depth_dependent_smoothing); - ne.compute(*normals_cloud); + int ind = h*width+w; + pcl::PointXYZRGBA & p = cloud->points[ind]; + p.b = rgbdata[3*ind+0]; + p.g = rgbdata[3*ind+1]; + p.r = rgbdata[3*ind+2]; + double z = idepth*double(depthdata[ind]); + if(z > 0){ + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + }else{ + p.x = NAN; + p.y = NAN; + p.z = NAN; + } + } + } + + //printf("%s LINE:%i\n",__FILE__,__LINE__); + pcl::IntegralImageNormalEstimation ne; + ne.setInputCloud(cloud); + + bool tune = false; + unsigned char * combidata; + cv::Mat combined; + + int NormalEstimationMethod = 0; + int MaxDepthChangeFactor = 20; + int NormalSmoothingSize = 7; + int depth_dependent_smoothing = 1; + + ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); + ne.setNormalSmoothingSize(NormalSmoothingSize); + ne.setDepthDependentSmoothing (depth_dependent_smoothing); + ne.compute(*normals_cloud); for(int w = 0; w < width; w++){ for(int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBA p = cloud->points[ind]; - pcl::Normal p2 = normals_cloud->points[ind]; - - - //if(w % 20 == 0 && h % 20 == 0){printf("%i %i -> point(%f %f %f) normal(%f %f %f)\n",w,h,p.x,p.y,p.z,p2.normal_x,p2.normal_y,p2.normal_z);} - - if(!isnan(p2.normal_x)){ - normalsdata[3*ind+0] = p2.normal_x; - normalsdata[3*ind+1] = p2.normal_y; - normalsdata[3*ind+2] = p2.normal_z; - }else{ - normalsdata[3*ind+0] = 2; - normalsdata[3*ind+1] = 2; - normalsdata[3*ind+2] = 2; - } - } - } - -// cv::Mat curvature; -// curvature.create(height,width,CV_8UC3); -// unsigned char * curvaturedata = (unsigned char *)curvature.data; -// for(int w = 0; w < width; w++){ -// for(int h = 0; h < height;h++){ -// int ind = h*width+w; -// pcl::Normal p2 = normals_cloud->points[ind]; -// //if(w % 5 == 0 && h % 5 == 0){printf("%i %i -> curvature %f\n",w,h,p2.curvature);} -// curvaturedata[3*ind+0] = 255.0*p2.curvature; -// curvaturedata[3*ind+1] = 255.0*p2.curvature; -// curvaturedata[3*ind+2] = 255.0*p2.curvature; -// } -// } -// cv::namedWindow( "curvature", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "curvature", curvature ); -// cv::waitKey(0); - - - - //printf("%s LINE:%i\n",__FILE__,__LINE__); - if(tune){ - combined.create(height,2*width,CV_8UC3); - combidata = (unsigned char *)combined.data; - - cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); - cv::namedWindow( "combined", cv::WINDOW_AUTOSIZE ); - - //cv::createTrackbar( "NormalEstimationMethod", "combined", &NormalEstimationMethod, 3, on_trackbar ); - //cv::createTrackbar( "MaxDepthChangeFactor", "combined", &MaxDepthChangeFactor, 1000, on_trackbar ); - //cv::createTrackbar( "NormalSmoothingSize", "combined", &NormalSmoothingSize, 100, on_trackbar ); - //cv::createTrackbar( "depth_dependent_smoothing", "combined", &depth_dependent_smoothing, 1, on_trackbar ); - - //while(true){ - - if(NormalEstimationMethod == 0){ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);} - if(NormalEstimationMethod == 1){ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);} - if(NormalEstimationMethod == 2){ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);} - if(NormalEstimationMethod == 3){ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);} - - ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); - ne.setNormalSmoothingSize(NormalSmoothingSize); - ne.setDepthDependentSmoothing (depth_dependent_smoothing); - ne.compute(*normals_cloud); - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::Normal p2 = normals_cloud->points[ind]; - if(!isnan(p2.normal_x)){ - normalsdata[3*ind+0] = p2.normal_x; - normalsdata[3*ind+1] = p2.normal_y; - normalsdata[3*ind+2] = p2.normal_z; - }else{ - normalsdata[3*ind+0] = 2; - normalsdata[3*ind+1] = 2; - normalsdata[3*ind+2] = 2; - } - } - } + int ind = h*width+w; + pcl::PointXYZRGBA p = cloud->points[ind]; + pcl::Normal p2 = normals_cloud->points[ind]; - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - int indn = h*2*width+(w+width); - int indc = h*2*width+(w); - combidata[3*indc+0] = rgbdata[3*ind+0]; - combidata[3*indc+1] = rgbdata[3*ind+1]; - combidata[3*indc+2] = rgbdata[3*ind+2]; - pcl::Normal p2 = normals_cloud->points[ind]; - if(!isnan(p2.normal_x)){ - combidata[3*indn+0] = 255.0*fabs(p2.normal_x); - combidata[3*indn+1] = 255.0*fabs(p2.normal_y); - combidata[3*indn+2] = 255.0*fabs(p2.normal_z); - }else{ - combidata[3*indn+0] = 255; - combidata[3*indn+1] = 255; - combidata[3*indn+2] = 255; - } - } - } - char buf [1024]; - sprintf(buf,"combined%i.png",int(id)); - cv::imwrite( buf, combined ); - printf("saving: %s\n",buf); - cv::imshow( "combined", combined ); - cv::waitKey(0); - //while(!updated){cv::waitKey(50);} - updated = false; - //} - } - - //pcl::PointCloud::Ptr cloud2 = getPCLcloud(); - pcl::OrganizedEdgeFromRGBNormals oed; - oed.setInputNormals (normals_cloud); - oed.setInputCloud (cloud); - oed.setDepthDisconThreshold (0.02); // 2cm -// oed.setHCCannyLowThreshold (0.4); -// oed.setHCCannyHighThreshold (1.1); - oed.setMaxSearchNeighbors (50); - pcl::PointCloud labels; - std::vector label_indices; - oed.compute (labels, label_indices); - -// pcl::PointCloud::Ptr occluding_edges (new pcl::PointCloud), -// occluded_edges (new pcl::PointCloud), -// boundary_edges (new pcl::PointCloud), -// high_curvature_edges (new pcl::PointCloud), -// rgb_edges (new pcl::PointCloud); - -// pcl::PointCloud::Ptr cloud3 (new pcl::PointCloud); -// pcl::copyPointCloud (*cloud2, label_indices[0].indices, *boundary_edges); -// pcl::copyPointCloud (*cloud2, label_indices[1].indices, *occluding_edges); -// pcl::copyPointCloud (*cloud2, label_indices[2].indices, *occluded_edges); -// pcl::copyPointCloud (*cloud2, label_indices[3].indices, *high_curvature_edges); -// pcl::copyPointCloud (*cloud2, label_indices[4].indices, *rgb_edges); - -// cv::Mat out = rgb.clone(); - - for(unsigned int i = 0; i < label_indices[1].indices.size(); i++){ - int ind = label_indices[1].indices[i]; - //depthedgesdata[ind] = 1; -// out.data[3*ind+0] =255; -// out.data[3*ind+1] =0; -// out.data[3*ind+2] =255; - } - -// for(unsigned int i = 0; i < label_indices[3].indices.size(); i++){ -// int ind = label_indices[3].indices[i]; -// depthedgesdata[ind] = 255; -// out.data[3*ind+0] =0; -// out.data[3*ind+1] =255; -// out.data[3*ind+2] =255; -// } - - - for(unsigned int i = 0; i < label_indices[4].indices.size(); i++){ - int ind = label_indices[4].indices[i]; + //if(w % 20 == 0 && h % 20 == 0){printf("%i %i -> point(%f %f %f) normal(%f %f %f)\n",w,h,p.x,p.y,p.z,p2.normal_x,p2.normal_y,p2.normal_z);} + + if(!isnan(p2.normal_x)){ + normalsdata[3*ind+0] = p2.normal_x; + normalsdata[3*ind+1] = p2.normal_y; + normalsdata[3*ind+2] = p2.normal_z; + }else{ + normalsdata[3*ind+0] = 2; + normalsdata[3*ind+1] = 2; + normalsdata[3*ind+2] = 2; + } + } + } + + // cv::Mat curvature; + // curvature.create(height,width,CV_8UC3); + // unsigned char * curvaturedata = (unsigned char *)curvature.data; + // for(int w = 0; w < width; w++){ + // for(int h = 0; h < height;h++){ + // int ind = h*width+w; + // pcl::Normal p2 = normals_cloud->points[ind]; + // //if(w % 5 == 0 && h % 5 == 0){printf("%i %i -> curvature %f\n",w,h,p2.curvature);} + // curvaturedata[3*ind+0] = 255.0*p2.curvature; + // curvaturedata[3*ind+1] = 255.0*p2.curvature; + // curvaturedata[3*ind+2] = 255.0*p2.curvature; + // } + // } + // cv::namedWindow( "curvature", cv::WINDOW_AUTOSIZE ); + // cv::imshow( "curvature", curvature ); + // cv::waitKey(0); + + + + //printf("%s LINE:%i\n",__FILE__,__LINE__); + if(tune){ + combined.create(height,2*width,CV_8UC3); + combidata = (unsigned char *)combined.data; + + cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); + cv::namedWindow( "combined", cv::WINDOW_AUTOSIZE ); + + //cv::createTrackbar( "NormalEstimationMethod", "combined", &NormalEstimationMethod, 3, on_trackbar ); + //cv::createTrackbar( "MaxDepthChangeFactor", "combined", &MaxDepthChangeFactor, 1000, on_trackbar ); + //cv::createTrackbar( "NormalSmoothingSize", "combined", &NormalSmoothingSize, 100, on_trackbar ); + //cv::createTrackbar( "depth_dependent_smoothing", "combined", &depth_dependent_smoothing, 1, on_trackbar ); + + //while(true){ + + if(NormalEstimationMethod == 0){ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);} + if(NormalEstimationMethod == 1){ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);} + if(NormalEstimationMethod == 2){ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);} + if(NormalEstimationMethod == 3){ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);} + + ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); + ne.setNormalSmoothingSize(NormalSmoothingSize); + ne.setDepthDependentSmoothing (depth_dependent_smoothing); + ne.compute(*normals_cloud); + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::Normal p2 = normals_cloud->points[ind]; + if(!isnan(p2.normal_x)){ + normalsdata[3*ind+0] = p2.normal_x; + normalsdata[3*ind+1] = p2.normal_y; + normalsdata[3*ind+2] = p2.normal_z; + }else{ + normalsdata[3*ind+0] = 2; + normalsdata[3*ind+1] = 2; + normalsdata[3*ind+2] = 2; + } + } + } + + + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int ind = h*width+w; + int indn = h*2*width+(w+width); + int indc = h*2*width+(w); + combidata[3*indc+0] = rgbdata[3*ind+0]; + combidata[3*indc+1] = rgbdata[3*ind+1]; + combidata[3*indc+2] = rgbdata[3*ind+2]; + pcl::Normal p2 = normals_cloud->points[ind]; + if(!isnan(p2.normal_x)){ + combidata[3*indn+0] = 255.0*fabs(p2.normal_x); + combidata[3*indn+1] = 255.0*fabs(p2.normal_y); + combidata[3*indn+2] = 255.0*fabs(p2.normal_z); + }else{ + combidata[3*indn+0] = 255; + combidata[3*indn+1] = 255; + combidata[3*indn+2] = 255; + } + } + } + char buf [1024]; + sprintf(buf,"combined%i.png",int(id)); + cv::imwrite( buf, combined ); + printf("saving: %s\n",buf); + cv::imshow( "combined", combined ); + cv::waitKey(0); + //while(!updated){cv::waitKey(50);} + updated = false; + //} + } + + //pcl::PointCloud::Ptr cloud2 = getPCLcloud(); + pcl::OrganizedEdgeFromRGBNormals oed; + oed.setInputNormals (normals_cloud); + oed.setInputCloud (cloud); + oed.setDepthDisconThreshold (0.02); // 2cm + // oed.setHCCannyLowThreshold (0.4); + // oed.setHCCannyHighThreshold (1.1); + oed.setMaxSearchNeighbors (50); + pcl::PointCloud labels; + std::vector label_indices; + oed.compute (labels, label_indices); + + // pcl::PointCloud::Ptr occluding_edges (new pcl::PointCloud), + // occluded_edges (new pcl::PointCloud), + // boundary_edges (new pcl::PointCloud), + // high_curvature_edges (new pcl::PointCloud), + // rgb_edges (new pcl::PointCloud); + + // pcl::PointCloud::Ptr cloud3 (new pcl::PointCloud); + // pcl::copyPointCloud (*cloud2, label_indices[0].indices, *boundary_edges); + // pcl::copyPointCloud (*cloud2, label_indices[1].indices, *occluding_edges); + // pcl::copyPointCloud (*cloud2, label_indices[2].indices, *occluded_edges); + // pcl::copyPointCloud (*cloud2, label_indices[3].indices, *high_curvature_edges); + // pcl::copyPointCloud (*cloud2, label_indices[4].indices, *rgb_edges); + + // cv::Mat out = rgb.clone(); + + for(unsigned int i = 0; i < label_indices[1].indices.size(); i++){ + int ind = label_indices[1].indices[i]; + //depthedgesdata[ind] = 1; + // out.data[3*ind+0] =255; + // out.data[3*ind+1] =0; + // out.data[3*ind+2] =255; + } + + // for(unsigned int i = 0; i < label_indices[3].indices.size(); i++){ + // int ind = label_indices[3].indices[i]; + // depthedgesdata[ind] = 255; + // out.data[3*ind+0] =0; + // out.data[3*ind+1] =255; + // out.data[3*ind+2] =255; + // } + + + for(unsigned int i = 0; i < label_indices[4].indices.size(); i++){ + int ind = label_indices[4].indices[i]; depthedgesdata[ind] = 255; -// out.data[3*ind+0] =0; -// out.data[3*ind+1] =255; -// out.data[3*ind+2] =0; - } + // out.data[3*ind+0] =0; + // out.data[3*ind+1] =255; + // out.data[3*ind+2] =0; + } -// cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "edges", out ); -// cv::waitKey(0); + // cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); + // cv::imshow( "edges", out ); + // cv::waitKey(0); - //show(true); - } + //show(true); + } if(true){ @@ -956,229 +1121,229 @@ exit(0); } } -if(false){ - show(false); + if(false){ + show(false); + + int * last = new int[width*height]; + for(int i = 0; i < width*height; i++){last[i] = 0;} + int current = 1; + + boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0, 0, 0); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + for(int w = 0; w < width; w+=5){ + for(int h = 0; h < height;h+=5){ + int ind = h*width+w; + + int edgetype = depthedgesdata[ind]; + if(edgetype != 0 && depthdata[ind] != 0){ + double xx = 0.001; + double xy = 0; + double xz = 0; + double yy = xx; + double yz = 0; + double zz = xx; + double sum = 0.001; + + double ww = 4.0; + double wh = 0; + double hh = 4.0; + double sum2 = 4.0; + + + double z = idepth*double(depthdata[ind]); + double x = (double(w) - cx) * z * ifx; + double y = (double(h) - cy) * z * ify; + + // for(int ww = 0; ww < width; ww++){ + // for(int hh = 0; hh < height;hh++){ + // int ind = hh*width+ww; + // pcl::PointXYZRGBA & p = cloud->points[ind]; + // p.b = 0; + // p.g = 255; + // p.r = 0; + // } + // } + + + + + std::vector todolistw; + todolistw.push_back(w); + std::vector todolisth; + todolisth.push_back(h); + current++; + last[ind] = current; + + cv::Mat rgbclone = rgb.clone(); + unsigned char * rgbclonedata = (unsigned char *)rgbclone.data; + + + cv::circle(rgbclone, cv::Point(w,h), 2, cv::Scalar(0,255,0)); + cv::imshow( "rgbclone", rgbclone ); + + + printf("ratio = ["); + double best_ratio = 0; + int tmp; + for(int go = 0; go < 100000 && go < todolistw.size(); go++){ + int cw = todolistw[go]; + int ch = todolisth[go]; + int ind = ch*width+cw; + + int dw = cw-w; + int dh = ch-h; + ww+=dw*dw; + wh+=dw*dh; + hh+=dh*dh; + sum2++; + + Eigen::Matrix2d mimg; + mimg(0,0) = ww/sum2; + mimg(0,1) = wh/sum2; + mimg(1,0) = wh/sum2; + mimg(1,1) = hh/sum2; + + Eigen::EigenSolver es(mimg); + double e1 = (es.eigenvalues())(0).real(); + double e2 = (es.eigenvalues())(1).real(); + double ratio = 1; + if(e1 > e2){ratio = e1/e2;} + if(e2 > e1){ratio = e2/e1;} + printf("%5.5f ",ratio); + + if(ratio < best_ratio){ + ww-=dw*dw; + wh-=dw*dh; + hh-=dh*dh; + sum2--; + //cv::circle(rgbclone, cv::Point(cw,ch), 2, cv::Scalar(255,0,255)); + continue; + } - int * last = new int[width*height]; - for(int i = 0; i < width*height; i++){last[i] = 0;} - int current = 1; + best_ratio = ratio; + // cout << "The mimg are:" << endl << mimg << endl; + // cout << "The eigenvalues of mimg are:" << endl << es.eigenvalues() << endl; + // printf("%f %f\n",e1,e2); + // cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl; - boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0, 0, 0); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); + // B2=(-cov[0][0]-cov[1][1])*(-cov[0][0]-cov[1][1]); + // c= (4*cov[0][0]*cov[1][1])-(4*cov[0][1]*cov[1][0]); - for(int w = 0; w < width; w+=5){ - for(int h = 0; h < height;h+=5){ - int ind = h*width+w; - int edgetype = depthedgesdata[ind]; - if(edgetype != 0 && depthdata[ind] != 0){ - double xx = 0.001; - double xy = 0; - double xz = 0; - double yy = xx; - double yz = 0; - double zz = xx; - double sum = 0.001; + // sq= sqrt(B2-c); - double ww = 4.0; - double wh = 0; - double hh = 4.0; - double sum2 = 4.0; + // B=(cov[0][0])+(cov[1][1]); - double z = idepth*double(depthdata[ind]); - double x = (double(w) - cx) * z * ifx; - double y = (double(h) - cy) * z * ify; + // r1=(B+sq)(.5); + // r2=(B-sq)(.5); -// for(int ww = 0; ww < width; ww++){ -// for(int hh = 0; hh < height;hh++){ -// int ind = hh*width+ww; -// pcl::PointXYZRGBA & p = cloud->points[ind]; -// p.b = 0; -// p.g = 255; -// p.r = 0; -// } -// } + // printf("\nThe eigenvalues are %f and %f", r1, r2); + double z2 = idepth*double(depthdata[ind]); + if(z2 > 0){ + double x2 = (double(cw) - cx) * z2 * ifx; + double y2 = (double(ch) - cy) * z2 * ify; - std::vector todolistw; - todolistw.push_back(w); - std::vector todolisth; - todolisth.push_back(h); - current++; - last[ind] = current; + double dx = x-x2; + double dy = y-y2; + double dz = z-z2; - cv::Mat rgbclone = rgb.clone(); - unsigned char * rgbclonedata = (unsigned char *)rgbclone.data; + xx += dx*dx; + xy += dx*dy; + xz += dx*dz; + yy += dy*dy; + yz += dy*dz; - cv::circle(rgbclone, cv::Point(w,h), 2, cv::Scalar(0,255,0)); - cv::imshow( "rgbclone", rgbclone ); + zz += dz*dz; + sum++; + } + //double z = idepth*double(depthdata[ind]); + rgbclonedata[3*ind+0] = 0; + rgbclonedata[3*ind+1] = 0; + rgbclonedata[3*ind+2] = 255; + + // cloud->points[ind].r = 255; + // cloud->points[ind].g = 0; + // cloud->points[ind].b = 0; + + int startw = std::max(int(0),cw-1); + int stopw = std::min(int(width-1),cw+1); + + int starth = std::max(int(0),ch-1); + int stoph = std::min(int(height-1),ch+1); + + for(int tw = startw; tw <= stopw; tw++){ + for(int th = starth; th <= stoph; th++){ + int ind2 = th*width+tw; + if(depthedgesdata[ind2] == edgetype && last[ind2] != current){ + todolistw.push_back(tw); + todolisth.push_back(th); + last[ind2] = current; + } + } + } - printf("ratio = ["); - double best_ratio = 0; - int tmp; - for(int go = 0; go < 100000 && go < todolistw.size(); go++){ - int cw = todolistw[go]; - int ch = todolisth[go]; - int ind = ch*width+cw; + } - int dw = cw-w; - int dh = ch-h; - ww+=dw*dw; - wh+=dw*dh; - hh+=dh*dh; - sum2++; + printf("];\n"); + ww /= sum2; + wh /= sum2; + hh /= sum2; Eigen::Matrix2d mimg; - mimg(0,0) = ww/sum2; - mimg(0,1) = wh/sum2; - mimg(1,0) = wh/sum2; - mimg(1,1) = hh/sum2; + mimg(0,0) = ww; + mimg(0,1) = wh; + mimg(1,0) = wh; + mimg(1,1) = hh; - Eigen::EigenSolver es(mimg); - double e1 = (es.eigenvalues())(0).real(); - double e2 = (es.eigenvalues())(1).real(); - double ratio = 1; - if(e1 > e2){ratio = e1/e2;} - if(e2 > e1){ratio = e2/e1;} - printf("%5.5f ",ratio); + Eigen::EigenSolver es(mimg); + cout << "The mimg are:" << endl << mimg << endl; + cout << "The eigenvalues of mimg are:" << endl << es.eigenvalues() << endl; + cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl; - if(ratio < best_ratio){ - ww-=dw*dw; - wh-=dw*dh; - hh-=dh*dh; - sum2--; - //cv::circle(rgbclone, cv::Point(cw,ch), 2, cv::Scalar(255,0,255)); - continue; - } + //cv::circle(rgbclone, cv::Point(w,h), 3, cv::Scalar(0,255,0)); - best_ratio = ratio; -// cout << "The mimg are:" << endl << mimg << endl; -// cout << "The eigenvalues of mimg are:" << endl << es.eigenvalues() << endl; -// printf("%f %f\n",e1,e2); -// cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl; - -// B2=(-cov[0][0]-cov[1][1])*(-cov[0][0]-cov[1][1]); -// c= (4*cov[0][0]*cov[1][1])-(4*cov[0][1]*cov[1][0]); - - -// sq= sqrt(B2-c); - - -// B=(cov[0][0])+(cov[1][1]); - -// r1=(B+sq)(.5); -// r2=(B-sq)(.5); - - -// printf("\nThe eigenvalues are %f and %f", r1, r2); - - - double z2 = idepth*double(depthdata[ind]); - if(z2 > 0){ - double x2 = (double(cw) - cx) * z2 * ifx; - double y2 = (double(ch) - cy) * z2 * ify; - - double dx = x-x2; - double dy = y-y2; - double dz = z-z2; - - xx += dx*dx; - xy += dx*dy; - xz += dx*dz; - - yy += dy*dy; - yz += dy*dz; - - zz += dz*dz; - sum++; - } - - //double z = idepth*double(depthdata[ind]); - rgbclonedata[3*ind+0] = 0; - rgbclonedata[3*ind+1] = 0; - rgbclonedata[3*ind+2] = 255; - -// cloud->points[ind].r = 255; -// cloud->points[ind].g = 0; -// cloud->points[ind].b = 0; - - int startw = std::max(int(0),cw-1); - int stopw = std::min(int(width-1),cw+1); - - int starth = std::max(int(0),ch-1); - int stoph = std::min(int(height-1),ch+1); - - for(int tw = startw; tw <= stopw; tw++){ - for(int th = starth; th <= stoph; th++){ - int ind2 = th*width+tw; - if(depthedgesdata[ind2] == edgetype && last[ind2] != current){ - todolistw.push_back(tw); - todolisth.push_back(th); - last[ind2] = current; - } - } - } - - } - - - printf("];\n"); - ww /= sum2; - wh /= sum2; - hh /= sum2; - Eigen::Matrix2d mimg; - mimg(0,0) = ww; - mimg(0,1) = wh; - mimg(1,0) = wh; - mimg(1,1) = hh; - - Eigen::EigenSolver es(mimg); - cout << "The mimg are:" << endl << mimg << endl; - cout << "The eigenvalues of mimg are:" << endl << es.eigenvalues() << endl; - cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl; - - //cv::circle(rgbclone, cv::Point(w,h), 3, cv::Scalar(0,255,0)); + cv::imshow( "rgbclone", rgbclone ); + cv::waitKey(0); + /* - cv::imshow( "rgbclone", rgbclone ); - cv::waitKey(0); -/* + pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); + cloud2->width = width; + cloud2->height = height; + cloud2->points.resize(width*height); + //cloud->dense = false; - pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); - cloud2->width = width; - cloud2->height = height; - cloud2->points.resize(width*height); - //cloud->dense = false; + unsigned short * depthdata = (unsigned short *)depth.data; + unsigned char * rgbdata = (unsigned char *)rgb.data; + for(int w3 = 0; w3 < width; w3++){ + for(int h3 = 0; h3 < height;h3++){ + int ind3 = h3*width+w3; + pcl::PointXYZRGBA & p = cloud2->points[ind3]; + p.b = rgbdata[3*ind3+0];//255; + p.g = rgbdata[3*ind3+1];//255; + p.r = rgbdata[3*ind3+2];//255; + } + } - unsigned short * depthdata = (unsigned short *)depth.data; - unsigned char * rgbdata = (unsigned char *)rgb.data; - for(int w3 = 0; w3 < width; w3++){ - for(int h3 = 0; h3 < height;h3++){ - int ind3 = h3*width+w3; - pcl::PointXYZRGBA & p = cloud2->points[ind3]; - p.b = rgbdata[3*ind3+0];//255; - p.g = rgbdata[3*ind3+1];//255; - p.r = rgbdata[3*ind3+2];//255; - } - } + for(int go = 0; go < todolistw.size(); go++){ + int cw = todolistw[go]; + int ch = todolisth[go]; + int ind3 = ch*width+cw; - for(int go = 0; go < todolistw.size(); go++){ - int cw = todolistw[go]; - int ch = todolisth[go]; - int ind3 = ch*width+cw; - - pcl::PointXYZRGBA & p = cloud2->points[ind3]; - p.b = 0; - p.g = 0; - p.r = 255; - } + pcl::PointXYZRGBA & p = cloud2->points[ind3]; + p.b = 0; + p.g = 0; + p.r = 255; + } for(int w3 = 0; w3 < width; w3++){ for(int h3 = 0; h3 < height;h3++){ @@ -1197,100 +1362,100 @@ if(false){ } } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud"); - viewer->spin(); - */ - } - } - } -} + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud"); + viewer->spin(); + */ + } + } + } + } } RGBDFrame::~RGBDFrame(){ - rgb.release(); - normals.release(); - depth.release(); - depthedges.release(); + rgb.release(); + normals.release(); + depth.release(); + depthedges.release(); if(labels != 0){delete[] labels; labels = 0;} } void RGBDFrame::show(bool stop){ - cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); - cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); cv::imshow( "normals", normals ); - cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", depth ); - cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges ); - if(stop){ cv::waitKey(0);}else{ cv::waitKey(30);} + cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); + cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); cv::imshow( "normals", normals ); + cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", depth ); + cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges ); + if(stop){ cv::waitKey(0);}else{ cv::waitKey(30);} } pcl::PointCloud::Ptr RGBDFrame::getPCLcloud(){ - unsigned char * rgbdata = (unsigned char *)rgb.data; - unsigned short * depthdata = (unsigned short *)depth.data; - - const unsigned int width = camera->width; const unsigned int height = camera->height; - const double idepth = camera->idepth_scale; - const double cx = camera->cx; const double cy = camera->cy; - const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy; - - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr normals (new pcl::PointCloud); - cloud->width = width; - cloud->height = height; - cloud->points.resize(width*height); - - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - double z = idepth*double(depthdata[ind]); - - pcl::PointXYZRGB p; - p.b = rgbdata[3*ind+0]; - p.g = rgbdata[3*ind+1]; - p.r = rgbdata[3*ind+2]; - if(z > 0){ - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - }else{ - p.x = NAN; - p.y = NAN; - p.z = NAN; - } - cloud->points[ind] = p; - } - } - - pcl::IntegralImageNormalEstimation ne; - ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); - ne.setMaxDepthChangeFactor(0.02f); - ne.setNormalSmoothingSize(10.0f); - ne.setInputCloud(cloud); - ne.compute(*normals); - - pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); - cloud_ptr->width = width; - cloud_ptr->height = height; - cloud_ptr->points.resize(width*height); - - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBNormal & p0 = cloud_ptr->points[ind]; - pcl::PointXYZRGB p1 = cloud->points[ind]; - pcl::Normal p2 = normals->points[ind]; - p0.x = p1.x; - p0.y = p1.y; - p0.z = p1.z; - p0.r = p1.r; - p0.g = p1.g; - p0.b = p1.b; - p0.normal_x = p2.normal_x; - p0.normal_y = p2.normal_y; - p0.normal_z = p2.normal_z; - } - } - return cloud_ptr; + unsigned char * rgbdata = (unsigned char *)rgb.data; + unsigned short * depthdata = (unsigned short *)depth.data; + + const unsigned int width = camera->width; const unsigned int height = camera->height; + const double idepth = camera->idepth_scale; + const double cx = camera->cx; const double cy = camera->cy; + const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(width*height); + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + double z = idepth*double(depthdata[ind]); + + pcl::PointXYZRGB p; + p.b = rgbdata[3*ind+0]; + p.g = rgbdata[3*ind+1]; + p.r = rgbdata[3*ind+2]; + if(z > 0){ + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + }else{ + p.x = NAN; + p.y = NAN; + p.z = NAN; + } + cloud->points[ind] = p; + } + } + + pcl::IntegralImageNormalEstimation ne; + ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); + ne.setMaxDepthChangeFactor(0.02f); + ne.setNormalSmoothingSize(10.0f); + ne.setInputCloud(cloud); + ne.compute(*normals); + + pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); + cloud_ptr->width = width; + cloud_ptr->height = height; + cloud_ptr->points.resize(width*height); + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::PointXYZRGBNormal & p0 = cloud_ptr->points[ind]; + pcl::PointXYZRGB p1 = cloud->points[ind]; + pcl::Normal p2 = normals->points[ind]; + p0.x = p1.x; + p0.y = p1.y; + p0.z = p1.z; + p0.r = p1.r; + p0.g = p1.g; + p0.b = p1.b; + p0.normal_x = p2.normal_x; + p0.normal_y = p2.normal_y; + p0.normal_z = p2.normal_z; + } + } + return cloud_ptr; } void RGBDFrame::savePCD(std::string path, Eigen::Matrix4d pose){ @@ -1331,69 +1496,69 @@ void RGBDFrame::savePCD(std::string path, Eigen::Matrix4d pose){ } void RGBDFrame::save(std::string path){ - //printf("saving frame %i to %s\n",id,path.c_str()); - - cv::imwrite( path+"_rgb.png", rgb ); - cv::imwrite( path+"_depth.png", depth ); - - unsigned long buffersize = 19*sizeof(double); - char* buffer = new char[buffersize]; - double * buffer_double = (double *)buffer; - unsigned long * buffer_long = (unsigned long *)buffer; - - int counter = 0; - buffer_double[counter++] = capturetime; - for(int i = 0; i < 4; i++){ - for(int j = 0; j < 4; j++){ - buffer_double[counter++] = pose(i,j); - } - } - buffer_long[counter++] = sweepid; - buffer_long[counter++] = camera->id; - std::ofstream outfile (path+"_data.txt",std::ofstream::binary); - outfile.write (buffer,buffersize); - outfile.close(); - delete[] buffer; + //printf("saving frame %i to %s\n",id,path.c_str()); + + cv::imwrite( path+"_rgb.png", rgb ); + cv::imwrite( path+"_depth.png", depth ); + + unsigned long buffersize = 19*sizeof(double); + char* buffer = new char[buffersize]; + double * buffer_double = (double *)buffer; + unsigned long * buffer_long = (unsigned long *)buffer; + + int counter = 0; + buffer_double[counter++] = capturetime; + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + buffer_double[counter++] = pose(i,j); + } + } + buffer_long[counter++] = sweepid; + buffer_long[counter++] = camera->id; + std::ofstream outfile (path+"_data.txt",std::ofstream::binary); + outfile.write (buffer,buffersize); + outfile.close(); + delete[] buffer; } RGBDFrame * RGBDFrame::load(Camera * cam, std::string path){ - printf("RGBDFrame * RGBDFrame::load(Camera * cam, std::string path)\n"); - - std::streampos size; - char * buffer; - char buf [1024]; - std::string datapath = path+"_data.txt"; - std::ifstream file (datapath, std::ios::in | std::ios::binary | std::ios::ate); - if (file.is_open()){ - size = file.tellg(); - buffer = new char [size]; - file.seekg (0, std::ios::beg); - file.read (buffer, size); - file.close(); - - double * buffer_double = (double *)buffer; - unsigned long * buffer_long = (unsigned long *)buffer; - - int counter = 0; - double capturetime = buffer_double[counter++]; - Eigen::Matrix4d pose; - for(int i = 0; i < 4; i++){ - for(int j = 0; j < 4; j++){ - pose(i,j) = buffer_double[counter++]; - } - } - int sweepid = buffer_long[counter++]; - int camera_id = buffer_long[counter++]; - - cv::Mat rgb = cv::imread(path+"_rgb.png", -1); // Read the file - cv::Mat depth = cv::imread(path+"_depth.png", -1); // Read the file - - RGBDFrame * frame = new RGBDFrame(cam,rgb,depth,capturetime,pose); - frame->sweepid = sweepid; - //printf("sweepid: %i",sweepid); - - return frame; - }else{printf("cant open %s\n",(path+"/data.txt").c_str());} + printf("RGBDFrame * RGBDFrame::load(Camera * cam, std::string path)\n"); + + std::streampos size; + char * buffer; + char buf [1024]; + std::string datapath = path+"_data.txt"; + std::ifstream file (datapath, std::ios::in | std::ios::binary | std::ios::ate); + if (file.is_open()){ + size = file.tellg(); + buffer = new char [size]; + file.seekg (0, std::ios::beg); + file.read (buffer, size); + file.close(); + + double * buffer_double = (double *)buffer; + unsigned long * buffer_long = (unsigned long *)buffer; + + int counter = 0; + double capturetime = buffer_double[counter++]; + Eigen::Matrix4d pose; + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + pose(i,j) = buffer_double[counter++]; + } + } + int sweepid = buffer_long[counter++]; + int camera_id = buffer_long[counter++]; + + cv::Mat rgb = cv::imread(path+"_rgb.png", -1); // Read the file + cv::Mat depth = cv::imread(path+"_depth.png", -1); // Read the file + + RGBDFrame * frame = new RGBDFrame(cam,rgb,depth,capturetime,pose); + frame->sweepid = sweepid; + //printf("sweepid: %i",sweepid); + + return frame; + }else{printf("cant open %s\n",(path+"/data.txt").c_str());} } } From c8fe87c1a06862b66ae23aae9412821f07869900 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Thu, 11 Aug 2016 10:45:23 +0200 Subject: [PATCH 058/255] Squashed all the bugs --- .../benchmark/src/benchmark_visualization.cpp | 2 +- .../dynamic_retrieval.h | 22 +++++++++++++--- quasimodo/quasimodo_msgs/CMakeLists.txt | 1 + .../quasimodo_msgs/msg/retrieval_query.msg | 4 +++ quasimodo/quasimodo_msgs/srv/insert_model.srv | 4 +-- .../create_object_search_observation.py | 2 ++ .../scripts/insert_object_server.py | 17 +++++++----- .../scripts/retrieve_object_search.py | 2 ++ .../src/quasimodo_retrieval_node.cpp | 26 ++++++++++++++++--- 9 files changed, 65 insertions(+), 15 deletions(-) diff --git a/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp b/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp index bf8ad6e9..4069926d 100644 --- a/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp +++ b/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp @@ -247,7 +247,7 @@ cv::Mat render_image(CloudT::Ptr& cloud, const Eigen::Matrix4f& T, const Eigen:: cv::Mat make_image(std::vector& results, const Eigen::Matrix4f& room_transform, vector& sweep_paths, const std::vector& optional_text) { - pair sizes = make_pair(1, 10);//get_similar_sizes(results.size()); + pair sizes = make_pair(1, std::min(10, int(results.size())));//get_similar_sizes(results.size()); int width = 200; int height = 200; diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index a85edad5..fd4d2fd1 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -125,7 +125,6 @@ std::vector get_retrieved_paths(const std::vector strs; @@ -134,8 +133,9 @@ std::vector get_retrieved_paths(const std::vector get_retrieved_paths(const std::vectorsurfel_cloud, *surfel_cloud); pcl::io::savePCDFileBinary(temp_path.string(), *surfel_cloud); pcl::io::savePCDFileBinary((temp_path.parent_path() / "convex_segments" / "segment.pcd").string(), *surfel_cloud); - retrieved_paths.push_back(temp_path.parent_path() / "convex_segments" / "segment.pcd"); + + if (kind == uri_kind::mongodb && !message->removed_at.empty()) { + retrieved_paths.push_back(boost::filesystem::path()); + } + else { + retrieved_paths.push_back(temp_path.parent_path() / "convex_segments" / "segment.pcd"); + ++loaded_mongodb_results; + } // [0] - database, [1] - collection, [2] - mongodb object id @@ -294,6 +301,10 @@ query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, VocabularyT& vt, const boost::filesystem::path& vocabulary_path, const vocabulary_summary& summary, bool verbose = false, uri_kind kind = uri_kind::all) { + if (verbose) { + std::cout << "Entering query_vocabulary ..." << std::endl; + } + // we need to cache the vt if we are to do this multiple times if (vt.empty()) { if (verbose) { @@ -329,6 +340,11 @@ query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, VocabularyT& vt, }), results.end()); results.resize(std::min(nbr_query, results.size())); + if (verbose) { + std::cout << "Exiting query_vocabulary with " << results.size() << " nbr results ..." << std::endl; + } + + return results; } diff --git a/quasimodo/quasimodo_msgs/CMakeLists.txt b/quasimodo/quasimodo_msgs/CMakeLists.txt index 07462af5..cb186800 100644 --- a/quasimodo/quasimodo_msgs/CMakeLists.txt +++ b/quasimodo/quasimodo_msgs/CMakeLists.txt @@ -68,6 +68,7 @@ add_service_files( visualize_query.srv transform_cloud.srv mask_pointclouds.srv + insert_model.srv ) ## Generate actions in the 'action' folder diff --git a/quasimodo/quasimodo_msgs/msg/retrieval_query.msg b/quasimodo/quasimodo_msgs/msg/retrieval_query.msg index ba94f84d..bef27724 100644 --- a/quasimodo/quasimodo_msgs/msg/retrieval_query.msg +++ b/quasimodo/quasimodo_msgs/msg/retrieval_query.msg @@ -1,3 +1,6 @@ +uint64 ALL_QUERY = 0 +uint64 MONGODB_QUERY = 1 +uint64 METAROOM_QUERY = 2 sensor_msgs/PointCloud2 cloud sensor_msgs/Image image sensor_msgs/Image depth @@ -5,3 +8,4 @@ sensor_msgs/Image mask sensor_msgs/CameraInfo camera int32 number_query geometry_msgs/Transform room_transform +uint64 query_kind diff --git a/quasimodo/quasimodo_msgs/srv/insert_model.srv b/quasimodo/quasimodo_msgs/srv/insert_model.srv index cee7f313..46c3602f 100644 --- a/quasimodo/quasimodo_msgs/srv/insert_model.srv +++ b/quasimodo/quasimodo_msgs/srv/insert_model.srv @@ -2,7 +2,7 @@ uint64 INSERT=1 uint64 REMOVE=2 uint64 action quasimodo_msgs/model model -uint64 object_id +string object_id --- uint64 vocabulary_id -uint64 object_id +string object_id diff --git a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py index 3ef66e97..a58a6b03 100755 --- a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py +++ b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py @@ -109,6 +109,8 @@ def chron_callback(): new_obj = fused_world_state_object() new_obj.surfel_cloud = resp.processed_cloud new_obj.object_id = x.id + now = datetime.now() + new_obj.inserted_at = now.strftime("%Y-%m-%d %H:%M:%S") for obs, pose in zip(wo._observations, wo._poses): new_obj.transforms.append(Pose()) diff --git a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py index 55b95426..8f7e1385 100755 --- a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py +++ b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py @@ -23,7 +23,7 @@ from geometry_msgs.msg import Pose import time -def insert_model(): +def insert_model_cb(): msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') transforms = [] @@ -118,32 +118,37 @@ def insert_model(): resp = insert_modelResponse() resp.vocabulary_id = new_obj.vocabulary_id - resp.object_id = new_obj.object_id + resp.object_id = object_id return resp -def remove_model(req): +def remove_model_cb(req): msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') # msg_store.delete(req.object_id) # resp = insert_modelResponse() # resp.object_id = req.object_id - new_obj = msg_store.query_id(req.object_id, fused_world_state_object) + new_obj = msg_store.query_id(req.object_id, 'quasimodo_msgs/fused_world_state_object')[0] + #print new_obj now = datetime.now() new_obj.removed_at = now.strftime("%Y-%m-%d %H:%M:%S") msg_store.update_id(req.object_id, new_obj) + resp = insert_modelResponse() + resp.vocabulary_id = new_obj.vocabulary_id + resp.object_id = req.object_id + return resp def service_callback(req): if req.action == insert_modelRequest.REMOVE: - resp = remove_model(req) + resp = remove_model_cb(req) if not resp: print "Failed to remove model..." elif req.action == insert_modelRequest.INSERT: - resp = insert_model(req) + resp = insert_model_cb(req) if not resp: print "Failed to remove model..." else: diff --git a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py index b1742d72..751eb5eb 100755 --- a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py +++ b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py @@ -95,6 +95,8 @@ def retrieval_callback(object_id): query.mask = CvBridge().cv2_to_imgmsg(cv_mask, "mono8") #masks[0] query.number_query = 10 query.room_transform = transforms[0] + #query.query_kind = retrieval_query.ALL_QUERY + query.query_kind = retrieval_query.MONGODB_QUERY pub.publish(query) resp.processed_cloud.header.frame_id = "/map" diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index 340de280..d1f2375d 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -432,6 +432,10 @@ class retrieval_node { vector retrieved_clouds; vector sweep_paths; // we should retrieve more than we want and only keep the results with valid files on the system + // FIXME: add support for grouped vocabulary here also, + // all of the message types should support it, just a matter + // of going from the native types to ros types, vis server should also support it + // make the few thing which might be dependent templatized functions auto results = dynamic_object_retrieval::query_reweight_vocabulary((vocabulary_tree&)vt, features, 200, vocabulary_path, summary); // This is just to make sure that we have valid results even when some meta rooms have been deleted @@ -525,7 +529,23 @@ class retrieval_node { void run_retrieval(const quasimodo_msgs::retrieval_query::ConstPtr& query_msg) //const string& sweep_xml) { - cout << "Received query msg..." << endl; + cout << "Received query msg of kind " << query_msg->query_kind << "..." << endl; + + dynamic_object_retrieval::uri_kind kind; + switch (query_msg->query_kind) { + case quasimodo_msgs::retrieval_query::MONGODB_QUERY: + kind = dynamic_object_retrieval::uri_kind::mongodb; + break; + case quasimodo_msgs::retrieval_query::METAROOM_QUERY: + kind = dynamic_object_retrieval::uri_kind::metaroom; + break; + case quasimodo_msgs::retrieval_query::ALL_QUERY: + kind = dynamic_object_retrieval::uri_kind::all; + break; + default: + cout << "Invalid quasimodo_msgs::retrieval_query::query_kind option: " << query_msg->query_kind << endl; + return; + } pcl::PointCloud::Ptr normal_cloud(new pcl::PointCloud); pcl::fromROSMsg(query_msg->cloud, *normal_cloud); @@ -562,11 +582,11 @@ class retrieval_node { vector sweep_paths; //auto results = dynamic_object_retrieval::query_reweight_vocabulary(vt, refined_query, query_image, query_depth, // K, number_query, vocabulary_path, summary, false); - auto results = dynamic_object_retrieval::query_reweight_vocabulary((vocabulary_tree&)vt, features, 200, vocabulary_path, summary, true); + auto results = dynamic_object_retrieval::query_reweight_vocabulary((vocabulary_tree&)vt, features, 200, vocabulary_path, summary, true, kind); // This is just to make sure that we have valid results even when some meta rooms have been deleted size_t counter = 0; - while (counter < number_query) { + while (counter < std::min(number_query, int32_t(results.first.size()))) { // assume that everything in mongodb is kept if (!boost::filesystem::exists(results.first[counter].first)) { auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair::result_type>& v) { From a25bc45d5d539a143f73b32db2d84a928c50aed7 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Thu, 11 Aug 2016 14:34:58 +0200 Subject: [PATCH 059/255] Removed debug code, now only fetching new observations every 10 minutes from world_state database --- .../scripts/create_object_search_observation.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py index a58a6b03..4dbd0d73 100755 --- a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py +++ b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py @@ -22,7 +22,7 @@ from geometry_msgs.msg import Pose import time -UPDATE_INT_MINUTES = 100000.0 +UPDATE_INT_MINUTES = 10.0 def chron_callback(): @@ -156,7 +156,7 @@ def chron_callback(): print object_id - break + #break # if you only want to add one object From 6014309877a245dc6440c040c7f22aace9fefe92 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 11 Aug 2016 17:29:49 +0200 Subject: [PATCH 060/255] working on bilateral filter --- .../src/segment_room_pairs.cpp | 107 ++----- .../src/segmentationserver.cpp | 12 + .../quasimodo_models/src/core/RGBDFrame.cpp | 268 ++++++++++++++++-- quasimodo/quasimodo_msgs/CMakeLists.txt | 1 + ...segmentation.srv.txt => metaroom_pair.srv} | 0 .../quasimodo_msgs/srv/segment_model.srv | 1 + 6 files changed, 285 insertions(+), 104 deletions(-) rename quasimodo/quasimodo_msgs/srv/{quasimodo_metaroom_segmentation.srv.txt => metaroom_pair.srv} (100%) diff --git a/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp index db503095..d8f78c8e 100644 --- a/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp +++ b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp @@ -32,6 +32,7 @@ #include "quasimodo_msgs/fuse_models.h" #include "quasimodo_msgs/get_model.h" #include "quasimodo_msgs/segment_model.h" +#include "quasimodo_msgs/metaroom_pair.h" #include "ros/ros.h" #include @@ -60,101 +61,37 @@ #include "core/RGBDFrame.h" #include "Util/Util.h" +ros::ServiceClient segmentation_client; using namespace std; -//typedef pcl::PointXYZRGB PointType; -//typedef pcl::PointCloud Cloud; -//typedef typename Cloud::Ptr CloudPtr; -//typedef pcl::search::KdTree Tree; -//typedef semantic_map_load_utilties::DynamicObjectData ObjectData; - -//using pcl::visualization::PointCloudColorHandlerCustom; - -//std::vector< std::vector > rgbs; -//std::vector< std::vector > depths; -//std::vector< std::vector > masks; -//std::vector< std::vector >tfs; -//std::vector< std::vector > initposes; -//std::vector< std::vector< image_geometry::PinholeCameraModel > > cams; - -//pcl::visualization::PCLVisualizer* pg; -//boost::shared_ptr viewer; - -//std::vector models; -/* -reglib::Model * load2(std::string sweep_xml){ - int slash_pos = sweep_xml.find_last_of("/"); - std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; - printf("folder: %s\n",sweep_folder.c_str()); - - SimpleXMLParser parser; - SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); - -// projection -// 532.158936 0.000000 310.514310 0.000000 -// 0.000000 533.819214 236.842039 0.000000 -// 0.000000 0.000000 1.000000 0.000000 - - reglib::Model * sweepmodel = 0; - - std::vector current_room_frames; - for (size_t i=0; ifx = 532.158936; - cam->fy = 533.819214; - cam->cx = 310.514310; - cam->cy = 236.842039; - - - cout<<"Intermediate cloud size "<points.size()<frames.push_back(frame); - sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose); - sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); - } - } +bool segment_metaroom(quasimodo_msgs::metaroom_pair::Request & req, quasimodo_msgs::metaroom_pair::Response & res){ + printf("segment_metaroom\n"); + + reglib::Model * bg_model = quasimodo_brain::load_metaroom_model(req.background); + reglib::Model * fg_model = quasimodo_brain::load_metaroom_model(req.foreground); - //sweepmodel->recomputeModelPoints(); - printf("nr points: %i\n",sweepmodel->points.size()); + quasimodo_msgs::segment_model sm; + sm.request.backgroundmodel = quasimodo_brain::getModelMSG(bg_model); + sm.request.models.push_back(quasimodo_brain::getModelMSG(fg_model)); - models.push_back(sweepmodel); - return sweepmodel; + if (segmentation_client.call(sm)){ + //int model_id = mff.response.model_id; + }else{ROS_ERROR("Failed to call service segment_model");} + + bg_model->fullDelete(); + delete bg_model; + fg_model->fullDelete(); + delete fg_model; } -*/ + int main(int argc, char** argv){ ros::init(argc, argv, "segmentationserver_metaroom"); ros::NodeHandle n; -// ros::ServiceServer service = n.advertiseService("segment_model", segment_model); + segmentation_client = n.serviceClient("segment_model"); + ros::ServiceServer service = n.advertiseService("segment_metaroom", segment_metaroom); + ros::spin(); /* reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index 5217f5cf..4e47fda3 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -247,7 +247,19 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs while(cv::waitKey(50)!='q'){viewer->spinOnce();} } + res.backgroundmodel = req.backgroundmodel; + + + res.masks.resize(models.size()); for(unsigned int i = 0; i < models.size(); i++){ + for(unsigned int j = 0; j < models[i]->frames.size(); j++){ + cv_bridge::CvImage maskBridgeImage; + maskBridgeImage.image = dynamic[i][j]; + maskBridgeImage.encoding = "mono8"; + res.masks[i].images.push_back( *(maskBridgeImage.toImageMsg()) ); + } + + res.models.push_back(quasimodo_brain::getModelMSG(models[i])); models[i]->fullDelete(); delete models[i]; } diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 7492113e..cd31c9ab 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -233,6 +233,15 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float * gest = new float[width*height]; float * best = new float[width*height]; + float * zdx = new float[width*height]; + float * rdx = new float[width*height]; + float * gdx = new float[width*height]; + float * bdx = new float[width*height]; + + float * zdy = new float[width*height]; + float * rdy = new float[width*height]; + float * gdy = new float[width*height]; + float * bdy = new float[width*height]; float * znext = new float[width*height]; float * rnext = new float[width*height]; @@ -240,14 +249,18 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float * bnext = new float[width*height]; for(int i = 0; i < width*height; i++){ - zbase[i] = zest[i] = idepth*double(depthdata[i]); - bbase[i] = best[i] = float(rgbdata[3*i+0])/256.0; - gbase[i] = gest[i] = float(rgbdata[3*i+1])/256.0; - rbase[i] = rest[i] = float(rgbdata[3*i+2])/256.0; + zbase[i] = zest[i] = idepth*double(depthdata[i]); + bbase[i] = best[i] = float(rgbdata[3*i+0])/256.0; + gbase[i] = gest[i] = float(rgbdata[3*i+1])/256.0; + rbase[i] = rest[i] = float(rgbdata[3*i+2])/256.0; + zdx[i] = zdy[i] = 0; + rdx[i] = rdy[i] = 0; + gdx[i] = gdy[i] = 0; + bdx[i] = bdy[i] = 0; } for(int it = 0; true; it++){ - if(it % 1 == 0){ + if(it % 100 == 0){ for(int w = 0; w < width; w++){ for(int h = 0; h < height;h++){ int ind = h*width+w; @@ -262,18 +275,80 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt p.y = 0; p.z = 0; } - p.b = best[ind]*256.0; - p.g = gest[ind]*256.0; - p.r = rest[ind]*256.0; + p.b = best[ind]*256.0; + p.g = gest[ind]*256.0; + p.r = rest[ind]*256.0; } } viewer->removeAllPointClouds(); viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); viewer->spin(); + + //for(int w = 0; w < width; w++){ + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++) + { + //int h= height/2; + int ind = h*width+w; + pcl::PointXYZRGBA & p = cloud->points[ind]; + + double z1 = zest[ind]; + double x1 = (double(w) - cx) * z1 * ifx; + double y1 = (double(h) - cy) * z1 * ify; + Eigen::Vector3d point1 (x1,y1,z1); + + double z2 = zest[ind]+zdx[ind]; + double x2 = (double(w+1) - cx) * z2 * ifx; + double y2 = (double(h) - cy) * z2 * ify; + Eigen::Vector3d point2 (x2,y2,z2); + + double z3 = zest[ind]+zdy[ind]; + double x3 = (double(w) - cx) * z2 * ifx; + double y3 = (double(h+1) - cy) * z2 * ify; + Eigen::Vector3d point3 (x3,y3,z3); + + Eigen::Vector3d normal = (point2-point1).cross(point3-point1); + normal.normalize(); + + float r = 0.5*(normal(0)+1.0); + float g = 0.5*(normal(1)+1.0); + float b = 0.5*(normal(2)+1.0); + + p.b = b*255.0;//(2.0*normal(2)-1.0)*255.0; + p.g = g*255.0;//(2.0*normal(1)-1.0)*255.0; + p.r = r*255.0;//(2.0*normal(0)-1.0)*255.0; + +// printf("%4.4i -> x1: %3.3f %3.3f %3.3f x2: %3.3f %3.3f %3.3f normal: %3.3f %3.3f %3.3f shifted: %3.3f %3.3f %3.3f rgb: %3.3i %3.3i %3.3i\n",w,point2(0)-point1(0),point2(1)-point1(1),point2(2)-point1(2), point3(0)-point1(0),point3(1)-point1(1),point3(2)-point1(2), normal(0),normal(1),normal(2), 0.5*(normal(0)+1.0),0.5*(normal(1)+1.0),0.5*(normal(2)+1.0), p.r,p.g,p.b); + +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); +// viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud"); +// viewer->spin(); + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud"); + viewer->spin(); + + } + float sumeL = 0; + float sumeR = 0; + float sumeU = 0; + float sumeD = 0; + + float sumeLpred = 0; + float sumeRpred = 0; + float sumeUpred = 0; + float sumeDpred = 0; + float angleVar = 100*pow(0.97,it); + printf("angleVar: %15.15f\n",angleVar); + for(int w = 1; w < width-1; w++){ for(int h = 1; h < height-1;h++){ int iM = h*width+w; @@ -282,13 +357,32 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt int iU = iM-width; int iD = iM+width; - float zMbase = zbase[iM]; - float zM = zest[iM]; - float zL = zest[iL]; - float zR = zest[iR]; - float zU = zest[iU]; - float zD = zest[iD]; - + //Z + float zMbase = zbase[iM]; + float zM = zest[iM]; + float zL = zest[iL]; + float zR = zest[iR]; + float zU = zest[iU]; + float zD = zest[iD]; + + float zMdx = zdx[iM]; + float zLdx = zdx[iL]; + float zRdx = zdx[iR]; + float zUdx = zdx[iU]; + float zDdx = zdx[iD]; + + float zMdy = zdy[iM]; + float zLdy = zdy[iL]; + float zRdy = zdy[iR]; + float zUdy = zdy[iU]; + float zDdy = zdy[iD]; + + float zLpred = zL-zLdx; + float zRpred = zR+zRdx; + float zUpred = zU-zUdy; + float zDpred = zD+zDdy; + + //R float rMbase = rbase[iM]; float rM = rest[iM]; float rL = rest[iL]; @@ -296,6 +390,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float rU = rest[iU]; float rD = rest[iD]; + //G float gMbase = gbase[iM]; float gM = gest[iM]; float gL = gest[iL]; @@ -303,6 +398,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float gU = gest[iU]; float gD = gest[iD]; + //B float bMbase = bbase[iM]; float bM = best[iM]; float bL = best[iL]; @@ -310,14 +406,148 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float bU = best[iU]; float bD = best[iD]; + //Diffs - float dzL = zM-zL; - float dzR = zM-zR; - float dzU = zM-zU; - float dzD = zM-zD; + float dzL2 = (zM-zL)*(zM-zL)/(zM*zM*zM*zM+zL*zL*zL*zL); + float dzL2pred = (zM-zLpred)*(zM-zLpred)/(zM*zM*zM*zM+zL*zL*zL*zL); + + float drL = rM-rL; + float dgL = gM-gL; + float dbL = bM-bL; + + float dzR2 = (zM-zR)*(zM-zR)/(zM*zM*zM*zM+zR*zR*zR*zR); + float dzR2pred = (zM-zRpred)*(zM-zRpred)/(zM*zM*zM*zM+zR*zR*zR*zR); + float drR = rM-rR; + float dgR = gM-gR; + float dbR = bM-bR; + + float dzU2 = (zM-zU)*(zM-zU)/(zM*zM*zM*zM+zU*zU*zU*zU); + float dzU2pred = (zM-zUpred)*(zM-zUpred)/(zM*zM*zM*zM+zU*zU*zU*zU); + float drU = rM-rU; + float dgU = gM-gU; + float dbU = bM-bU; + + float dzD2 = (zM-zD)*(zM-zD)/(zM*zM*zM*zM+zD*zD*zD*zD); + float dzD2pred = (zM-zDpred)*(zM-zDpred)/(zM*zM*zM*zM+zD*zD*zD*zD); + float drD = rM-rD; + float dgD = gM-gD; + float dbD = bM-bD; + + int itthresh = 20000; + if(it < itthresh || dzL2 < dzL2pred){ + dzL2pred = dzL2; + zLpred = zL; + } + + if(it < itthresh || dzR2 < dzR2pred){ + dzR2pred = dzR2; + zRpred = zR; + } + + if(it < itthresh || dzU2 < dzU2pred){ + dzU2pred = dzU2; + zUpred = zU; + } + + if(it < itthresh || dzD2 < dzD2pred){ + dzD2pred = dzD2; + zDpred = zD; + } + + float zangleL = (zLdx-zMdx)*(zLdx-zMdx)+(zLdy-zMdy)*(zLdy-zMdy); + float zangleR = (zRdx-zMdx)*(zRdx-zMdx)+(zRdy-zMdy)*(zRdy-zMdy); + float zangleU = (zUdx-zMdx)*(zUdx-zMdx)+(zUdy-zMdy)*(zUdy-zMdy); + float zangleD = (zDdx-zMdx)*(zDdx-zMdx)+(zDdy-zMdy)*(zDdy-zMdy); + + + //Weights + float distVar = 0.01*0.01; + + float weightM = 1.0*float(zMbase > 0); + float weightL = exp(-0.5*dzL2pred/distVar)*exp(-0.5*zangleL/angleVar); + float weightR = exp(-0.5*dzR2pred/distVar)*exp(-0.5*zangleR/angleVar); + float weightU = exp(-0.5*dzU2pred/distVar)*exp(-0.5*zangleU/angleVar); + float weightD = exp(-0.5*dzD2pred/distVar)*exp(-0.5*zangleD/angleVar); + + + + float sumW = weightM+weightL+weightR+weightU+weightD; + if(sumW > 0){ + sumeL += dzL2*weightL; + sumeR += dzR2*weightR; + sumeU += dzU2*weightU; + sumeD += dzD2*weightD; + + sumeLpred += dzL2pred*weightL; + sumeRpred += dzR2pred*weightR; + sumeUpred += dzU2pred*weightU; + sumeDpred += dzD2pred*weightD; + + if(isnan(sumeLpred)){ + printf("w: %i h:%i\n",w,h); + + printf("L:: val: %3.3f dx: %3.3f dy: %3.3f pred: %3.3f\n",zL,zLdx,zLdy,zLpred); + printf("normal sums: %10.10f %10.10f %10.10f %10.10f\npredic sums: %10.10f %10.10f %10.10f %10.10f\n",sumeL,sumeR,sumeU,sumeD,sumeLpred,sumeRpred,sumeUpred,sumeDpred); + exit(0); + } + + float znextM = (zM*weightM+zLpred*weightL+zRpred*weightR+zUpred*weightU+zDpred*weightD)/sumW; + znext[iM] = znextM; + if(weightL + weightR > 0){ + zdx[iM] = (weightL*(zL-znextM) + weightR*(znextM-zR))/(weightL + weightR); + } + + if(weightU + weightD > 0){ + zdy[iM] = (weightU*(zU-znextM) + weightD*(znextM-zD))/(weightU + weightD); + } + /* + if(fabs(znext[iM] - zMbase)/sqrt(zMbase*zMbase*zMbase*zMbase + znext[iM]*znext[iM]*znext[iM]*znext[iM]) > 0.01 && znext[iM] < 2.0){ + + printf("new value: %3.3f\n",znext[iM]); + printf("value M: %3.3f L %3.3f R %3.3f U %3.3f D %3.3f\n",zM,zL,zR,zU,zD); + printf("weight M: %3.3f L %3.3f R %3.3f U %3.3f D %3.3f\n",weightM,weightL,weightR,weightU,weightD); + + + pcl::PointXYZRGBA & p = cloud->points[iM]; + p.b = 0; + p.g = 255; + p.r = 0; + + viewer->removeAllShapes(); + viewer->addLine (cloud->points[iM],cloud->points[iL],(1-weightL),weightL,0,"L"); + viewer->addLine (cloud->points[iM],cloud->points[iR],(1-weightR),weightR,0,"R"); + viewer->addLine (cloud->points[iM],cloud->points[iU],(1-weightU),weightU,0,"U"); + viewer->addLine (cloud->points[iM],cloud->points[iD],(1-weightD),weightD,0,"D"); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + + p.b = 255;//best[iM]*256.0; + p.g = 0;//gest[iM]*256.0; + p.r = 255;//rest[iM]*256.0; + + } + */ + }else{ + znext[iM] = zest[iM]; + } } } + + printf("------\n"); + printf("normal sums: %10.10f %10.10f %10.10f %10.10f\npredic sums: %10.10f %10.10f %10.10f %10.10f\n",sumeL,sumeR,sumeU,sumeD,sumeLpred,sumeRpred,sumeUpred,sumeDpred); + +// std::memcpy(zest,znext,width*height*sizeof(float)); +// std::memcpy(rest,rnext,width*height*sizeof(float)); +// std::memcpy(gest,gnext,width*height*sizeof(float)); +// std::memcpy(best,bnext,width*height*sizeof(float)); + + float * ztmp = zest; zest = znext; znext = ztmp; +// float * rtmp = rest; rest = rnext; rnext = rtmp; +// float * gtmp = gest; gest = gnext; gnext = gtmp; +// float * btmp = best; best = bnext; bnext = btmp; } exit(0); } diff --git a/quasimodo/quasimodo_msgs/CMakeLists.txt b/quasimodo/quasimodo_msgs/CMakeLists.txt index e0e48f5d..7c1aef93 100644 --- a/quasimodo/quasimodo_msgs/CMakeLists.txt +++ b/quasimodo/quasimodo_msgs/CMakeLists.txt @@ -65,6 +65,7 @@ add_service_files( simple_query_cloud.srv visualize_query.srv segment_model.srv + metaroom_pair.srv ) ## Generate actions in the 'action' folder diff --git a/quasimodo/quasimodo_msgs/srv/quasimodo_metaroom_segmentation.srv.txt b/quasimodo/quasimodo_msgs/srv/metaroom_pair.srv similarity index 100% rename from quasimodo/quasimodo_msgs/srv/quasimodo_metaroom_segmentation.srv.txt rename to quasimodo/quasimodo_msgs/srv/metaroom_pair.srv diff --git a/quasimodo/quasimodo_msgs/srv/segment_model.srv b/quasimodo/quasimodo_msgs/srv/segment_model.srv index 097bffb1..8a9e071b 100644 --- a/quasimodo/quasimodo_msgs/srv/segment_model.srv +++ b/quasimodo/quasimodo_msgs/srv/segment_model.srv @@ -6,3 +6,4 @@ uint64 models_refine --- model backgroundmodel model[] models +image_array[] masks From 1c5dbec157cda4e08b367552cff06ed5ce2bbef3 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 12 Aug 2016 06:13:06 +0200 Subject: [PATCH 061/255] working on bilateral filter --- .../quasimodo_models/src/core/RGBDFrame.cpp | 62 +++++++++++-------- 1 file changed, 36 insertions(+), 26 deletions(-) diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index cd31c9ab..e2291c69 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -260,7 +260,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } for(int it = 0; true; it++){ - if(it % 100 == 0){ + if(it % 10 == 0){ for(int w = 0; w < width; w++){ for(int h = 0; h < height;h++){ int ind = h*width+w; @@ -433,26 +433,26 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float dgD = gM-gD; float dbD = bM-bD; - int itthresh = 20000; - if(it < itthresh || dzL2 < dzL2pred){ - dzL2pred = dzL2; - zLpred = zL; - } + int itthresh = 20; + if(it < itthresh || dzL2 < dzL2pred){ + dzL2pred = dzL2; + zLpred = zL; + } - if(it < itthresh || dzR2 < dzR2pred){ - dzR2pred = dzR2; - zRpred = zR; - } + if(it < itthresh || dzR2 < dzR2pred){ + dzR2pred = dzR2; + zRpred = zR; + } - if(it < itthresh || dzU2 < dzU2pred){ - dzU2pred = dzU2; - zUpred = zU; - } + if(it < itthresh || dzU2 < dzU2pred){ + dzU2pred = dzU2; + zUpred = zU; + } - if(it < itthresh || dzD2 < dzD2pred){ - dzD2pred = dzD2; - zDpred = zD; - } + if(it < itthresh || dzD2 < dzD2pred){ + dzD2pred = dzD2; + zDpred = zD; + } float zangleL = (zLdx-zMdx)*(zLdx-zMdx)+(zLdy-zMdy)*(zLdy-zMdy); float zangleR = (zRdx-zMdx)*(zRdx-zMdx)+(zRdy-zMdy)*(zRdy-zMdy); @@ -460,17 +460,28 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float zangleD = (zDdx-zMdx)*(zDdx-zMdx)+(zDdy-zMdy)*(zDdy-zMdy); - //Weights - float distVar = 0.01*0.01; + //Weights + float distVar = 0.1*0.1; + float colVar = 0.1*0.1; - float weightM = 1.0*float(zMbase > 0); - float weightL = exp(-0.5*dzL2pred/distVar)*exp(-0.5*zangleL/angleVar); - float weightR = exp(-0.5*dzR2pred/distVar)*exp(-0.5*zangleR/angleVar); - float weightU = exp(-0.5*dzU2pred/distVar)*exp(-0.5*zangleU/angleVar); - float weightD = exp(-0.5*dzD2pred/distVar)*exp(-0.5*zangleD/angleVar); + float diffZ = fabs(zM-zMbase)/(0.001*zMbase*zMbase); + float weightM = pow(diffZ,4)*0.01*float(zMbase > 0);//exp(-0.5*(zM-zMbase)*(zM-zMbase)/distVar);//1.0*float(zMbase > 0); + float weightL = exp(-0.5*(dzL2/distVar + drL*drL/colVar + dgL*dgL/colVar + dbL*dbL/colVar));//*exp(-0.5*zangleL/angleVar); + float weightR = exp(-0.5*(dzR2/distVar + drR*drR/colVar + dgR*dgR/colVar + dbR*dbR/colVar));//*exp(-0.5*zangleR/angleVar); + float weightU = exp(-0.5*(dzU2/distVar + drU*drU/colVar + dgU*dgU/colVar + dbU*dbU/colVar));//*exp(-0.5*zangleU/angleVar); + float weightD = exp(-0.5*(dzD2/distVar + drD*drD/colVar + dgD*dgD/colVar + dbD*dbD/colVar));//*exp(-0.5*zangleD/angleVar); + if(w == width/2 && h == height/2){ + printf("w: %i h:%i\n",w,h); + printf("M: %3.3f Base: %3.3f\n",zM,zMbase); + printf("L:: val: %3.3f dx: %3.3f dy: %3.3f pred: %3.3f\n",zL,zLdx,zLdy,zLpred); + printf("R:: val: %3.3f dx: %3.3f dy: %3.3f pred: %3.3f\n",zR,zRdx,zRdy,zRpred); + printf("U:: val: %3.3f dx: %3.3f dy: %3.3f pred: %3.3f\n",zU,zUdx,zUdy,zUpred); + printf("D:: val: %3.3f dx: %3.3f dy: %3.3f pred: %3.3f\n",zD,zDdx,zDdy,zDpred); + } + float sumW = weightM+weightL+weightR+weightU+weightD; if(sumW > 0){ sumeL += dzL2*weightL; @@ -485,7 +496,6 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt if(isnan(sumeLpred)){ printf("w: %i h:%i\n",w,h); - printf("L:: val: %3.3f dx: %3.3f dy: %3.3f pred: %3.3f\n",zL,zLdx,zLdy,zLpred); printf("normal sums: %10.10f %10.10f %10.10f %10.10f\npredic sums: %10.10f %10.10f %10.10f %10.10f\n",sumeL,sumeR,sumeU,sumeD,sumeLpred,sumeRpred,sumeUpred,sumeDpred); exit(0); From de523504dc303ef58136256e7377a8b4ab1d6180 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 12 Aug 2016 17:12:04 +0200 Subject: [PATCH 062/255] integrating segmentation --- quasimodo/quasimodo_brain/CMakeLists.txt | 4 + quasimodo/quasimodo_brain/src/Util/Util.cpp | 232 ++++++++++++++++++ quasimodo/quasimodo_brain/src/Util/Util.h | 2 + .../src/segment_room_pairs.cpp | 4 + .../src/segmentationserver.cpp | 47 ++-- .../quasimodo_brain/src/test_segment.cpp | 81 +++--- .../src/test_segment_room_pairs.cpp | 96 ++++++++ .../quasimodo_models/include/core/RGBDFrame.h | 2 +- .../include/modelupdater/ModelUpdater.h | 2 + .../quasimodo_models/src/core/RGBDFrame.cpp | 82 +++++-- .../quasimodo_models/src/model/Model.cpp | 7 +- .../src/modelupdater/ModelUpdater.cpp | 61 +++-- .../src/registration/MassRegistrationPPR2.cpp | 32 ++- .../quasimodo_msgs/srv/segment_model.srv | 3 +- 14 files changed, 553 insertions(+), 102 deletions(-) create mode 100644 quasimodo/quasimodo_brain/src/test_segment_room_pairs.cpp diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 93fdba7c..9a5f2c38 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -148,6 +148,10 @@ add_executable( segment_room_pairs src/segment_room_pairs.cpp) add_dependencies( segment_room_pairs roscpp quasimodo_msgs_generate_messages_cpp) target_link_libraries( segment_room_pairs quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +add_executable( test_segment_room_pairs src/test_segment_room_pairs.cpp) +add_dependencies( test_segment_room_pairs roscpp quasimodo_msgs_generate_messages_cpp) +target_link_libraries( test_segment_room_pairs quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + add_executable( segmentationserver src/segmentationserver.cpp) add_dependencies( segmentationserver roscpp quasimodo_msgs_generate_messages_cpp) target_link_libraries( segmentationserver quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 7790cc2c..1fc05ccb 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -187,4 +187,236 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ return sweepmodel; } +void segment(reglib::Model * bg, std::vector< reglib::Model * > models, +std::vector< std::vector< cv::Mat > > & internal, +std::vector< std::vector< cv::Mat > > & external, +std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ + boost::shared_ptr viewer; + if(debugg){ + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); + viewer->addCoordinateSystem(0.01); + viewer->setBackgroundColor(0.0,0.0,0.0); + } + + reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.25); + massregmod->timeout = 1200; + massregmod->viewer = viewer; + massregmod->visualizationLvl = 0; + massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massregmod->nomask = true; + massregmod->stopval = 0.0001; + + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + + if(models.size() > 0 && bg->frames.size() > 0){ + std::vector cpmod; + + bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + cpmod.push_back(Eigen::Matrix4d::Identity());//,models.front()->relativeposes.front().inverse() * bg->relativeposes.front()); + massregmod->addModel(bg); + + for(int j = 0; j < models.size(); j++){ + models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + cpmod.push_back(bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); + massregmod->addModel(models[j]); + } + + reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); + + for(int j = 0; j < models.size(); j++){ + Eigen::Matrix4d change = mfrmod.poses[j+1] * cpmod[j+1].inverse(); + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + } + } + }else if(models.size() > 1){ + std::vector cpmod; + + for(int j = 0; j < models.size(); j++){ + models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); + massregmod->addModel(models[j]); + } + + reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); + for(int j = 0; j < models.size(); j++){ + Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + } + } + } + + delete massregmod; + + + std::vector bgcp; + std::vector bgcf; + std::vector bgmask; + + for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ + bgcp.push_back(bg->relativeposes[k]); + bgcf.push_back(bg->frames[k]); + bgmask.push_back(bg->modelmasks[k]->getMask()); + } + + for(int j = 0; j < models.size(); j++){ + reglib::Model * model = models[j]; + + std::vector masks; + for(unsigned int i = 0; i < model->frames.size(); i++){ + reglib::RGBDFrame * frame = model->frames[i]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + masks.push_back(mask); + } + + mu->computeMovingDynamicStatic(bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions + + + std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions + std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,false);//Determine external occlusions + std::vector dynamic_masks; + for(unsigned int i = 0; i < model->frames.size(); i++){ + reglib::RGBDFrame * frame = model->frames[i]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[i].data); + for(unsigned int k = 0; k < cam->height * cam->width;k++){ + if(externalmaskdata[k] == 0 && internalmaskdata[k] != 0 ){ + maskdata[k] = 255; + }else{ + maskdata[k] = 0; + } + } + + dynamic_masks.push_back(mask); + +// cv::imshow( "rgb", frame->rgb ); +// cv::imshow( "internal_masks", internal_masks[i] ); +// cv::imshow( "externalmask", external_masks[i] ); +// cv::imshow( "dynamic_mask", dynamic_masks[i] ); +// cv::waitKey(0); + } + + internal.push_back(internal_masks); + external.push_back(external_masks); + dynamic.push_back(dynamic_masks); + } + +/* + for(unsigned int i = 0; visualization && i < models.size(); i++){ + std::vector internal_masks = internal[i]; + std::vector external_masks = external[i]; + std::vector dynamic_masks = dynamic[i]; + reglib::Model * model = models[i]; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + for(unsigned int j = 0; j < model->frames.size(); j++){ + reglib::RGBDFrame * frame = model->frames[j]; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + reglib::Camera * camera = frame->camera; + + cv::imshow( "rgb", frame->rgb ); + cv::imshow( "internal_masks", internal_masks[j] ); + cv::imshow( "externalmask", external_masks[j] ); + cv::imshow( "dynamic_mask", dynamic_masks[j] ); + cv::waitKey(100); + + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); + unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); + + Eigen::Matrix4d p = model->relativeposes[j]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + pcl::PointXYZRGBNormal point; + point.x = m00*x + m01*y + m02*z + m03; + point.y = m10*x + m11*y + m12*z + m13; + point.z = m20*x + m21*y + m22*z + m23; + + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; + + if(dynamicmaskdata[ind] != 0){ + point.b = 0; + point.g = 255; + point.r = 0; + }else if(internalmaskdata[ind] == 0){ + point.b = 0; + point.g = 0; + point.r = 255; + } + + cloud->points.push_back(point); + } + } + } + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + while(cv::waitKey(50)!='q'){viewer->spinOnce();} + } + + res.backgroundmodel = req.backgroundmodel; + + + res.masks.resize(models.size()); + for(unsigned int i = 0; i < models.size(); i++){ + for(unsigned int j = 0; j < models[i]->frames.size(); j++){ + cv_bridge::CvImage maskBridgeImage; + maskBridgeImage.image = dynamic[i][j]; + maskBridgeImage.encoding = "mono8"; + res.masks[i].images.push_back( *(maskBridgeImage.toImageMsg()) ); + } + + res.models.push_back(quasimodo_brain::getModelMSG(models[i])); + models[i]->fullDelete(); + delete models[i]; + } + models.clear(); + */ + + delete reg; + delete mu; +} + } diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index a8cff3a5..a502fc70 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -44,6 +44,8 @@ std::vector getRegisteredViewPoses(const std::string& poses_fil reglib::Model * load_metaroom_model(std::string sweep_xml); +void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg = false); + } #endif diff --git a/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp index d8f78c8e..7af6ddd1 100644 --- a/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp +++ b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp @@ -69,6 +69,9 @@ using namespace std; bool segment_metaroom(quasimodo_msgs::metaroom_pair::Request & req, quasimodo_msgs::metaroom_pair::Response & res){ printf("segment_metaroom\n"); + printf("background: %s\n",req.background.c_str()); + printf("foreground: %s\n",req.foreground.c_str()); + reglib::Model * bg_model = quasimodo_brain::load_metaroom_model(req.background); reglib::Model * fg_model = quasimodo_brain::load_metaroom_model(req.foreground); @@ -84,6 +87,7 @@ bool segment_metaroom(quasimodo_msgs::metaroom_pair::Request & req, quasimodo_m delete bg_model; fg_model->fullDelete(); delete fg_model; + return true; } int main(int argc, char** argv){ diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index 4e47fda3..fa8e33fa 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -38,6 +38,9 @@ boost::shared_ptr viewer; //Update background //How? + +//std::vector + bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs::segment_model::Response & res){ printf("segment_model\n"); std::vector< reglib::Model * > models; @@ -47,6 +50,10 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs reglib::Model * bg = quasimodo_brain::getModelFromMSG(req.backgroundmodel); + std::vector< std::vector< cv::Mat > > internal; + std::vector< std::vector< cv::Mat > > external; + std::vector< std::vector< cv::Mat > > dynamic; +/* reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.25); massregmod->timeout = 1200; massregmod->viewer = viewer; @@ -117,9 +124,7 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs } - std::vector< std::vector< cv::Mat > > internal; - std::vector< std::vector< cv::Mat > > external; - std::vector< std::vector< cv::Mat > > dynamic; + for(int j = 0; j < models.size(); j++){ reglib::Model * model = models[j]; @@ -135,7 +140,7 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs masks.push_back(mask); } - std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,true);//Determine self occlusions + std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,false);//Determine external occlusions std::vector dynamic_masks; for(unsigned int i = 0; i < model->frames.size(); i++){ @@ -169,9 +174,11 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs external.push_back(external_masks); dynamic.push_back(dynamic_masks); } +*/ + quasimodo_brain::segment(bg,models,internal,external,dynamic); - for(unsigned int i = 0; i < models.size(); i++){ + for(unsigned int i = 0; visualization && i < models.size(); i++){ std::vector internal_masks = internal[i]; std::vector external_masks = external[i]; std::vector dynamic_masks = dynamic[i]; @@ -185,11 +192,11 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs reglib::Camera * camera = frame->camera; - cv::imshow( "rgb", frame->rgb ); - cv::imshow( "internal_masks", internal_masks[j] ); - cv::imshow( "externalmask", external_masks[j] ); - cv::imshow( "dynamic_mask", dynamic_masks[j] ); - cv::waitKey(100); +// cv::imshow( "rgb", frame->rgb ); +// cv::imshow( "internal_masks", internal_masks[j] ); +// cv::imshow( "externalmask", external_masks[j] ); +// cv::imshow( "dynamic_mask", dynamic_masks[j] ); +// cv::waitKey(100); unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); @@ -244,19 +251,26 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs } viewer->removeAllPointClouds(); viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - while(cv::waitKey(50)!='q'){viewer->spinOnce();} + viewer->spin(); + //while(cv::waitKey(50)!='q'){viewer->spinOnce();} } res.backgroundmodel = req.backgroundmodel; - res.masks.resize(models.size()); + res.dynamicmasks.resize(models.size()); + res.internalmasks.resize(models.size()); for(unsigned int i = 0; i < models.size(); i++){ for(unsigned int j = 0; j < models[i]->frames.size(); j++){ - cv_bridge::CvImage maskBridgeImage; - maskBridgeImage.image = dynamic[i][j]; - maskBridgeImage.encoding = "mono8"; - res.masks[i].images.push_back( *(maskBridgeImage.toImageMsg()) ); + cv_bridge::CvImage dynamicmaskBridgeImage; + dynamicmaskBridgeImage.image = dynamic[i][j]; + dynamicmaskBridgeImage.encoding = "mono8"; + res.dynamicmasks[i].images.push_back( *(dynamicmaskBridgeImage.toImageMsg()) ); + + cv_bridge::CvImage internalmaskBridgeImage; + internalmaskBridgeImage.image = internal[i][j]; + internalmaskBridgeImage.encoding = "mono8"; + res.internalmasks[i].images.push_back( *(internalmaskBridgeImage.toImageMsg()) ); } res.models.push_back(quasimodo_brain::getModelMSG(models[i])); @@ -269,6 +283,7 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs } int main(int argc, char** argv){ + ros::init(argc, argv, "segmentationserver"); ros::NodeHandle n; diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index e827cede..e969fb4b 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -100,7 +100,7 @@ reglib::Model * load2(std::string sweep_xml){ reglib::Model * sweepmodel = 0; std::vector current_room_frames; - for (size_t i=0; irecomputeModelPoints(); - printf("nr points: %i\n",sweepmodel->points.size()); + //sweepmodel->recomputeModelPoints(); + //printf("nr points: %i\n",sweepmodel->points.size()); models.push_back(sweepmodel); return sweepmodel; } +//segment(reglib::Model * bg, std::vector< reglib::Model * > models, bool debugg) + int main(int argc, char** argv){ - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0.5, 0, 0.5); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); +// reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + +// viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); +// viewer->setBackgroundColor (0.5, 0, 0.5); +// viewer->addCoordinateSystem (1.0); +// viewer->initCameraParameters (); for(int ar = 1; ar < argc; ar++){ string overall_folder = std::string(argv[ar]); @@ -169,46 +172,62 @@ int main(int argc, char** argv){ } } - //Not needed if metaroom well calibrated - reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); - bgmassreg->timeout = 20; - bgmassreg->viewer = viewer; - bgmassreg->use_surface = true; - bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = 0; - bgmassreg->maskstep = 10; - bgmassreg->nomaskstep = 10; - bgmassreg->nomask = true; - bgmassreg->stopval = 0.0005; - bgmassreg->setData(models.front()->frames,models.front()->modelmasks); - reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); - delete bgmassreg; - - +// //Not needed if metaroom well calibrated +// reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); +// bgmassreg->timeout = 20; +// bgmassreg->viewer = viewer; +// bgmassreg->use_surface = true; +// bgmassreg->use_depthedge = false; +// bgmassreg->visualizationLvl = 0; +// bgmassreg->maskstep = 10; +// bgmassreg->nomaskstep = 10; +// bgmassreg->nomask = true; +// bgmassreg->stopval = 0.0005; +// bgmassreg->setData(models.front()->frames,models.front()->modelmasks); +// reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); +// delete bgmassreg; + + +/* ros::init(argc, argv, "test_segment"); ros::NodeHandle n; ros::ServiceClient segmentation_client = n.serviceClient("segment_model"); + for(unsigned int i = 1; i < models.size(); i++){ quasimodo_msgs::segment_model sm; sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); - - if(i > 0){ - sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]); - } + if(i > 0){sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]);} if (segmentation_client.call(sm)){//Build model from frame - //int model_id = mff.response.model_id; printf("segmented: %i\n",i); }else{ROS_ERROR("Failed to call service segment_model");} } - //ros::spin(); +*/ + + for(unsigned int i = 1; i < models.size(); i++){ + std::vector< reglib::Model * > foreground; + foreground.push_back(models[i]); + std::vector< std::vector< cv::Mat > > internal; + std::vector< std::vector< cv::Mat > > external; + std::vector< std::vector< cv::Mat > > dynamic; + quasimodo_brain::segment(models[i-1],foreground,internal,external,dynamic); + +// quasimodo_msgs::segment_model sm; +// sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); +// if(i > 0){sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]);} + +// if (segmentation_client.call(sm)){//Build model from frame +// printf("segmented: %i\n",i); +// }else{ROS_ERROR("Failed to call service segment_model");} + } - delete reg; for(size_t j = 0; j < models.size(); j++){ models[j]->fullDelete(); delete models[j]; } + + //delete reg; printf("done\n"); return 0; } diff --git a/quasimodo/quasimodo_brain/src/test_segment_room_pairs.cpp b/quasimodo/quasimodo_brain/src/test_segment_room_pairs.cpp new file mode 100644 index 00000000..9a63d813 --- /dev/null +++ b/quasimodo/quasimodo_brain/src/test_segment_room_pairs.cpp @@ -0,0 +1,96 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include +#include + +#include +#include + +#include "eigen_conversions/eigen_msg.h" +#include "tf_conversions/tf_eigen.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" +#include "metaroom_xml_parser/simple_summary_parser.h" + +#include + +#include "ros/ros.h" +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" +#include "quasimodo_msgs/segment_model.h" +#include "quasimodo_msgs/metaroom_pair.h" + +#include "ros/ros.h" +#include +#include +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" + +#include +#include + +#include "modelupdater/ModelUpdater.h" +#include "core/RGBDFrame.h" +#include "Util/Util.h" + +ros::ServiceClient segmentation_client; + + +using namespace std; + + +int main(int argc, char** argv){ + ros::init(argc, argv, "test_segmentationserver_metaroom"); + ros::NodeHandle n; + ros::ServiceClient segmentation_client = n.serviceClient("segment_metaroom"); + + vector room_xmls; + for(int ar = 1; ar < argc; ar++){ + string overall_folder = std::string(argv[ar]); + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + printf("sweep_xmls\n"); + for (auto sweep_xml : sweep_xmls) { + printf("sweep_xml: %s\n",sweep_xml.c_str()); + room_xmls.push_back(sweep_xml); + //load2(sweep_xml); + } + } + + quasimodo_msgs::metaroom_pair sm; + sm.request.background = std::string(room_xmls[0]); + sm.request.foreground = std::string(room_xmls[1]); + + if (segmentation_client.call(sm)){ + //int model_id = mff.response.model_id; + }else{ROS_ERROR("Failed to call service segment_model");} + + return 0; +} diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 24ee427e..4670a3bb 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -34,7 +34,7 @@ namespace reglib Eigen::Matrix4d pose; int sweepid; - float * rgbdata; + //float * rgbdata; cv::Mat rgb; cv::Mat depth; diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index c2c3ca36..636be73f 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -97,6 +97,8 @@ namespace reglib virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks); virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg = false); + virtual void computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); + virtual OcclusionScore computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1, bool debugg = false); virtual OcclusionScore computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp = Matrix4d::Identity(), int step = 1, bool debugg = false); virtual OcclusionScore computeOcclusionScore(Model * model1, Model * model2, Matrix4d rp = Matrix4d::Identity(),int step = 1, bool debugg = false); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index e2291c69..2517bb08 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -87,8 +87,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt sweepid = -1; id = RGBDFrame_id_counter++; camera = camera_; - rgb = rgb_.clone(); - depth = depth_.clone(); + rgb = rgb_; + depth = depth_; capturetime = capturetime_; pose = pose_; @@ -126,13 +126,23 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt //printf("%s LINE:%i\n",__FILE__,__LINE__); unsigned short * depthdata = (unsigned short *)depth.data; - unsigned char * rgbdata = (unsigned char *)rgb.data; + unsigned char * rgbdata = (unsigned char *)rgb.data; + +// rgbdata = 0; + +// rgbdata = new float[3*width*height]; +// for(int i = 0; i < 3*width*height; i++){ +// rgbdata[i] = float(img_rgbdata[i]); +// } depthedges.create(height,width,CV_8UC1); unsigned char * depthedgesdata = (unsigned char *)depthedges.data; for(int i = 0; i < width*height; i++){ depthedgesdata[i] = 0; } + + //rgbdata + /* double t = 0.01; for(int w = 0; w < width; w++){ @@ -212,7 +222,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt // src = dst.clone(); // } - if(true){ + if(false){ pcl::PointCloud::Ptr cloud (new pcl::PointCloud); cloud->width = width; cloud->height = height; @@ -260,7 +270,10 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } for(int it = 0; true; it++){ - if(it % 10 == 0){ + if(it % 50 == 0){ + + + for(int w = 0; w < width; w++){ for(int h = 0; h < height;h++){ int ind = h*width+w; @@ -333,7 +346,25 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud"); viewer->spin(); - + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::PointXYZRGBA & p = cloud->points[ind]; + double z = zest[ind]; + if(z > 0){ + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + }else{ + p.x = 0; + p.y = 0; + p.z = 0; + } + p.b = best[ind]*256.0; + p.g = gest[ind]*256.0; + p.r = rest[ind]*256.0; + } + } } @@ -433,7 +464,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float dgD = gM-gD; float dbD = bM-bD; - int itthresh = 20; + int itthresh = 2000; if(it < itthresh || dzL2 < dzL2pred){ dzL2pred = dzL2; zLpred = zL; @@ -461,17 +492,24 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt //Weights - float distVar = 0.1*0.1; - float colVar = 0.1*0.1; + float distVar = 0.01*0.01; + float colVar = 0.1*0.1; float diffZ = fabs(zM-zMbase)/(0.001*zMbase*zMbase); - float weightM = pow(diffZ,4)*0.01*float(zMbase > 0);//exp(-0.5*(zM-zMbase)*(zM-zMbase)/distVar);//1.0*float(zMbase > 0); + float weightM = pow(diffZ,2)*0.01*float(zMbase > 0);//exp(-0.5*(zM-zMbase)*(zM-zMbase)/distVar);//1.0*float(zMbase > 0); float weightL = exp(-0.5*(dzL2/distVar + drL*drL/colVar + dgL*dgL/colVar + dbL*dbL/colVar));//*exp(-0.5*zangleL/angleVar); float weightR = exp(-0.5*(dzR2/distVar + drR*drR/colVar + dgR*dgR/colVar + dbR*dbR/colVar));//*exp(-0.5*zangleR/angleVar); float weightU = exp(-0.5*(dzU2/distVar + drU*drU/colVar + dgU*dgU/colVar + dbU*dbU/colVar));//*exp(-0.5*zangleU/angleVar); float weightD = exp(-0.5*(dzD2/distVar + drD*drD/colVar + dgD*dgD/colVar + dbD*dbD/colVar));//*exp(-0.5*zangleD/angleVar); + if((weightL > 0.4 && weightR < 0.2) || (weightL < 0.2 && weightR > 0.4) || (weightU > 0.4 && weightD < 0.2) || (weightU < 0.2 && weightD > 0.4)){ + pcl::PointXYZRGBA & p = cloud->points[iM]; + p.b = 0; + p.g = 255; + p.r = 0; + } + if(w == width/2 && h == height/2){ printf("w: %i h:%i\n",w,h); @@ -561,16 +599,19 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } exit(0); } - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - cloud->width = width; - cloud->height = height; - cloud->points.resize(width*height); - boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0, 0, 0); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); if(false){ + + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(width*height); + + boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0, 0, 0); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); // cv::Mat bilat; // cv::adaptiveBilateralFilter(rgb, bilat, cv::Size(21,21), 30); @@ -1123,10 +1164,6 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt // delete[] z_est; // delete[] col_est; // delete[] normal_est; - - - - } //printf("%s LINE:%i\n",__FILE__,__LINE__); @@ -1618,6 +1655,7 @@ RGBDFrame::~RGBDFrame(){ normals.release(); depth.release(); depthedges.release(); + //if(rgbdata != 0){delete[] rgbdata; rgbdata = 0;} if(labels != 0){delete[] labels; labels = 0;} } diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index fc3d3421..a633ba1e 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -35,6 +35,7 @@ Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask){ bool * maskvec = modelmask->maskvec; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + //float * rgbdata = frame->rgbdata; unsigned short * depthdata = (unsigned short *)(frame->depth.data); float * normalsdata = (float *)(frame->normals.data); @@ -135,9 +136,9 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr float pny = m10*dst_nx + m11*dst_ny + m12*dst_nz; float pnz = m20*dst_nx + m21*dst_ny + m22*dst_nz; - float pb = (float)(rgbdata[3*ind+0]); - float pg = (float)(rgbdata[3*ind+1]); - float pr = (float)(rgbdata[3*ind+2]); + float pb = (float)(rgbdata[3*dst_ind+0]); + float pg = (float)(rgbdata[3*dst_ind+1]); + float pr = (float)(rgbdata[3*dst_ind+2]); Vector3f pxyz (px ,py ,pz ); Vector3f pnxyz (pnx,pny,pnz); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index fc6d37ab..df6376df 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2054,6 +2054,20 @@ std::vector doInference(std::vector & prior, std::vector< std::vect return labels; } +void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ + int tot_nr_pixels = 0; + std::vector offsets; + + for(unsigned int i = 0; i < frames.size(); i++){ + offsets.push_back(tot_nr_pixels); + unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; + tot_nr_pixels += nr_pixels; + } + printf("tot_nr_pixels: %i\n",tot_nr_pixels); + exit(0); +} + + vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg){ std::vector newmasks; @@ -2306,7 +2320,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector global_labels = doInference(prior,connectionId,connectionStrength); + //std::vector global_labels = doInference(prior,connectionId,connectionStrength); for(unsigned int i = 0; i < interframe_connectionId.size(); i++){ for(unsigned int j = 0; j < interframe_connectionId[i].size(); j++){ @@ -2321,7 +2335,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vectordepth.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); +// unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); Camera * camera = frame->camera; const unsigned int width = camera->width; @@ -2329,35 +2343,35 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vectorcamera->width * frame->camera->height; - cv::Mat originalmask; - originalmask.create(height,width,CV_8UC1); - unsigned char * originaldata = (unsigned char *)(originalmask.data); +// cv::Mat originalmask; +// originalmask.create(height,width,CV_8UC1); +// unsigned char * originaldata = (unsigned char *)(originalmask.data); cv::Mat improvedmask; improvedmask.create(height,width,CV_8UC1); unsigned char * improveddata = (unsigned char *)(improvedmask.data); - cv::Mat diffmask; - diffmask.create(height,width,CV_8UC3); - unsigned char * diffdata = (unsigned char *)(diffmask.data); +// cv::Mat diffmask; +// diffmask.create(height,width,CV_8UC3); +// unsigned char * diffdata = (unsigned char *)(diffmask.data); for(unsigned int i = 0; i < nr_pixels;i++){ //internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); - originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph::SOURCE)); + //originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph::SOURCE)); improveddata[i] = 255.0*((depthdata[i] == 0) || (improvedglobal_labels[i+offset] == gc::Graph::SOURCE)); - if(improveddata[i] < originaldata[i]){ - diffdata[3*i+0] = 0; - diffdata[3*i+1] = 255; - diffdata[3*i+2] = 0; - }else if(improveddata[i] > originaldata[i]){ - diffdata[3*i+0] = 0; - diffdata[3*i+1] = 0; - diffdata[3*i+2] = 255; - }else{ - diffdata[3*i+0] = 0; - diffdata[3*i+1] = 0; - diffdata[3*i+2] = 0; - } +// if(improveddata[i] < originaldata[i]){ +// diffdata[3*i+0] = 0; +// diffdata[3*i+1] = 255; +// diffdata[3*i+2] = 0; +// }else if(improveddata[i] > originaldata[i]){ +// diffdata[3*i+0] = 0; +// diffdata[3*i+1] = 0; +// diffdata[3*i+2] = 255; +// }else{ +// diffdata[3*i+0] = 0; +// diffdata[3*i+1] = 0; +// diffdata[3*i+2] = 0; +// } } @@ -2376,7 +2390,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector ModelUpdater::computeDynamicObject(vector bgcp, vectorspin(); viewer->removeAllPointClouds(); + */ } return newmasks; diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 40b9f35d..75894c64 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -9,6 +9,8 @@ namespace reglib { +//#define domultithread + MassRegistrationPPR2::MassRegistrationPPR2(double startreg, bool visualize){ type = PointToPlane; //type = PointToPoint; @@ -536,7 +538,11 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, int nr_a double good = 0; double bad = 0; -#pragma omp parallel for num_threads(8) + + +#if defdomultithread + #pragma omp parallel for num_threads(8) +#endif for(unsigned int k = 0; k < nr_ap; ++k) { int prev = matchid[k]; double qp [3]; @@ -581,7 +587,11 @@ double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, i double good = 0; double bad = 0; - #pragma omp parallel for num_threads(8) + + +#if defdomultithread + #pragma omp parallel for num_threads(8) +#endif for(unsigned int k = 0; k < nr_ap; ++k) { const double & src_x = ap[3*k+0]; const double & src_y = ap[3*k+1]; @@ -1547,7 +1557,11 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ // const int nrdn = depthedge_nr_neighbours+1; -// #pragma omp parallel for num_threads(8) +// + +//#if defdomultithread +// #pragma omp parallel for num_threads(8) +//#endif // for(int c = 0; c < depthedge_count; c++){ // size_t ret_index[nrdn]; // double out_dist_sqr[nrdn]; @@ -2828,7 +2842,11 @@ if(useOrb){ float di = 10.0; float pi = 10.0; - #pragma omp parallel for num_threads(8) + + +#if defdomultithread + #pragma omp parallel for num_threads(8) +#endif for(unsigned int src_k = 0; src_k < src_nr_ap; ++src_k) { const double & src_x = src_ap[3*src_k+0]; const double & src_y = src_ap[3*src_k+1]; @@ -2968,7 +2986,11 @@ if(useSurf){ const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); - #pragma omp parallel for num_threads(8) + + +#if defdomultithread + #pragma omp parallel for num_threads(8) +#endif for(unsigned int src_k = 0; src_k < src_nr_ap; ++src_k) { const double & src_x = src_ap[3*src_k+0]; const double & src_y = src_ap[3*src_k+1]; diff --git a/quasimodo/quasimodo_msgs/srv/segment_model.srv b/quasimodo/quasimodo_msgs/srv/segment_model.srv index 8a9e071b..fd17c862 100644 --- a/quasimodo/quasimodo_msgs/srv/segment_model.srv +++ b/quasimodo/quasimodo_msgs/srv/segment_model.srv @@ -6,4 +6,5 @@ uint64 models_refine --- model backgroundmodel model[] models -image_array[] masks +image_array[] dynamicmasks +image_array[] internalmasks From 578671496044e4fe922eebb44bbb4f33d20c4acb Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 16 Aug 2016 07:45:04 +0200 Subject: [PATCH 063/255] trying new joint class inference --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 7 +- .../quasimodo_brain/src/test_segment.cpp | 42 +- .../include/modelupdater/ModelUpdater.h | 2 +- .../src/modelupdater/ModelUpdater.cpp | 381 ++++++++++++++++++ 4 files changed, 407 insertions(+), 25 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 1fc05ccb..bde87ce6 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -187,15 +187,12 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ return sweepmodel; } -void segment(reglib::Model * bg, std::vector< reglib::Model * > models, -std::vector< std::vector< cv::Mat > > & internal, -std::vector< std::vector< cv::Mat > > & external, -std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ +void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ boost::shared_ptr viewer; if(debugg){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); viewer->addCoordinateSystem(0.01); - viewer->setBackgroundColor(0.0,0.0,0.0); + viewer->setBackgroundColor(1.0,1.0,1.0); } reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.25); diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index e969fb4b..690a3c96 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -157,10 +157,10 @@ int main(int argc, char** argv){ // reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); -// viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); -// viewer->setBackgroundColor (0.5, 0, 0.5); -// viewer->addCoordinateSystem (1.0); -// viewer->initCameraParameters (); +// viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); +// viewer->setBackgroundColor (0.5, 0, 0.5); +// viewer->addCoordinateSystem (1.0); +// viewer->initCameraParameters (); for(int ar = 1; ar < argc; ar++){ string overall_folder = std::string(argv[ar]); @@ -172,20 +172,24 @@ int main(int argc, char** argv){ } } -// //Not needed if metaroom well calibrated -// reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); -// bgmassreg->timeout = 20; -// bgmassreg->viewer = viewer; -// bgmassreg->use_surface = true; -// bgmassreg->use_depthedge = false; -// bgmassreg->visualizationLvl = 0; -// bgmassreg->maskstep = 10; -// bgmassreg->nomaskstep = 10; -// bgmassreg->nomask = true; -// bgmassreg->stopval = 0.0005; -// bgmassreg->setData(models.front()->frames,models.front()->modelmasks); -// reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); -// delete bgmassreg; + //Not needed if metaroom well calibrated + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); + bgmassreg->timeout = 20; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = false; + bgmassreg->visualizationLvl = 0; + bgmassreg->maskstep = 4; + bgmassreg->nomaskstep = 4; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->setData(models.front()->frames,models.front()->modelmasks); + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(models.front()->relativeposes); + delete bgmassreg; + + for(unsigned int i = 0; i < models.size(); i++){ + models[i]->relativeposes = bgmfr.poses; + } /* @@ -211,7 +215,7 @@ int main(int argc, char** argv){ std::vector< std::vector< cv::Mat > > internal; std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; - quasimodo_brain::segment(models[i-1],foreground,internal,external,dynamic); + quasimodo_brain::segment(models[i-1],foreground,internal,external,dynamic,true); // quasimodo_msgs::segment_model sm; // sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 636be73f..53353979 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -92,7 +92,7 @@ namespace reglib virtual void getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2); virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); - + virtual void getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, double debugg); virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength,double debugg = false); virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks); virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg = false); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index df6376df..bd9b64e2 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2054,6 +2054,186 @@ std::vector doInference(std::vector & prior, std::vector< std::vect return labels; } +void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, double debugg){ + + unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); + float * src_normalsdata = (float *)(frame1->normals.data); + + Camera * src_camera = frame1->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); + float * dst_normalsdata = (float *)(frame2->normals.data); + + Camera * dst_camera = frame2->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + //bool debugg = true; + pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); + if(debugg){ + + src_cloud->points.resize(src_width*src_height); + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + point.r = 255; + point.g = 0; + point.b = 0; + src_cloud->points[src_ind] = point; + } + } + } + + dst_cloud->points.resize(dst_width*dst_height); + for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + pcl::PointXYZRGBNormal point; + point.x = dst_x; + point.y = dst_y; + point.z = dst_z; + point.r = 0; + point.g = 0; + point.b = 255; + dst_cloud->points[dst_ind] = point; + } + } + } + } + + std::vector inds; + std::vector distances; + + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + double dst_noise = dst_z * dst_z; + double point_noise = src_z * src_z; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); + //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); + //d *= src_z;//compare_mul; + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + if(surface_angle > 0.9){ + overlaps[src_ind] = std::max(overlaps[src_ind],0.9); +// overlaps[src_ind]++; +// if(debugg){ +//// dst_cloud->points[dst_ind].r = 0; +//// dst_cloud->points[dst_ind].g = 255; +//// dst_cloud->points[dst_ind].b = 0; +// src_cloud->points[src_ind].r = 0; +// src_cloud->points[src_ind].g = 255; +// src_cloud->points[src_ind].b = 0; +// } + } + }else if(tz < dst_z){ + occlusions[src_ind] = std::max(occlusions[src_ind],0.9); + if(debugg){ + src_cloud->points[src_ind].r = 255; + src_cloud->points[src_ind].g = 0; + src_cloud->points[src_ind].b = 255; + } + } + } + } + } + } + } + + if(debugg){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); + viewer->spin(); + } +} + void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ int tot_nr_pixels = 0; std::vector offsets; @@ -2064,6 +2244,207 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector::Ptr cloud (new pcl::PointCloud); + + for(unsigned int i = 0; i < frames.size(); i++){ + int offset = offsets[i]; + RGBDFrame * frame = frames[i]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + float * normalsdata = (float *)(frame->normals.data); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + + unsigned int nr_pixels = width * height; + double * current_overlaps = new double[nr_pixels]; + double * current_occlusions = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + current_overlaps[j] = 0; + current_occlusions[j] = 0; + } + + for(unsigned int j = 0; j < frames.size(); j++){ + if(i == j){continue;} + Eigen::Matrix4d p = poses[i].inverse() * poses[j]; + getDynamicWeights(p.inverse(),frames[i], current_overlaps, current_occlusions, frames[j],false); + } + + + double * bg_overlaps = new double[nr_pixels]; + double * bg_occlusions = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + bg_overlaps[j] = 0; + bg_occlusions[j] = 0; + } + + for(unsigned int j = 0; j < bgcf.size(); j++){ + Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; + getDynamicWeights(p.inverse(),frames[i], bg_overlaps, bg_occlusions, bgcf[j],false); + } + + Eigen::Matrix4d p = poses[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + + float minprob = 0.01; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + float p_moving = current_occlusions[ind]; + float p_dynamic = std::max((bg_occlusions[ind]-p_moving)*current_overlaps[ind],0.0); + p_dynamic += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); + p_moving += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); + + float p_static = std::max(bg_overlaps[ind]-p_moving-p_dynamic,0.0); + + float leftover = 1-p_moving-p_dynamic-p_static; + float notMoving = current_overlaps[ind]; + + float bias = 0.0001; + float p_moving_leftover = -bias+(1-notMoving)*leftover/3.0; + float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/3.0; + float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/3.0; + + float p_moving_tot = (1.0-3.0*minprob)*(p_moving+p_moving_leftover)+minprob; + float p_dynamic_tot = (1.0-3.0*minprob)*(p_dynamic+p_dynamic_leftover)+minprob; + float p_static_tot = (1.0-3.0*minprob)*(p_static+p_static_leftover)+minprob; + + float norm = p_moving_tot+p_dynamic_tot+p_static_tot; + float tx = normalsdata[3*ind+0]; + float ty = normalsdata[3*ind+1]; + float tz = normalsdata[3*ind+2]; + p_moving_tot /= norm; + p_dynamic_tot/= norm; + p_static_tot /= norm; + + priors[3*current_point+0] = p_moving_tot; + priors[3*current_point+1] = p_dynamic_tot; + priors[3*current_point+2] = p_static_tot; + estimates[3*current_point+0] = priors[3*current_point+0]; + estimates[3*current_point+1] = priors[3*current_point+1]; + estimates[3*current_point+2] = priors[3*current_point+2]; + points[3*current_point+0] = m00*x + m01*y + m02*z + m03; + points[3*current_point+1] = m10*x + m11*y + m12*z + m13; + points[3*current_point+2] = m20*x + m21*y + m22*z + m23; + noise[current_point] = z*z; + colors[3*current_point+0] = rgbdata[3*ind+0]; + colors[3*current_point+1] = rgbdata[3*ind+1]; + colors[3*current_point+2] = rgbdata[3*ind+2]; + normals[3*current_point+0] = -2; + normals[3*current_point+1] = -2; + normals[3*current_point+2] = -2; + if(tx != 2){ + normals[3*current_point+0] = m00*tx + m01*ty + m02*tz; + normals[3*current_point+1] = m10*tx + m11*ty + m12*tz; + normals[3*current_point+2] = m20*tx + m21*ty + m22*tz; + } + current_point++; + + pcl::PointXYZRGBNormal point; + point.x = m00*x + m01*y + m02*z + m03; + point.y = m10*x + m11*y + m12*z + m13; + point.z = m20*x + m21*y + m22*z + m23; + point.b = p_moving_tot*255.0; + point.g = p_dynamic_tot*255.0; + point.r = p_static_tot*255.0; + + cloud->points.push_back(point); + } + } + } + +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); +// viewer->spin(); + + +// for(unsigned int w = 0; w < width;w++){ +// for(unsigned int h = 0; h < height;h++){ +// int ind = h*width+w; +// float z = idepth*float(depthdata[ind]); +// if(z > 0){ +// float x = (float(w) - cx) * z * ifx; +// float y = (float(h) - cy) * z * ify; + +// pcl::PointXYZRGBNormal point; +// point.x = m00*x + m01*y + m02*z + m03; +// point.y = m10*x + m11*y + m12*z + m13; +// point.z = m20*x + m21*y + m22*z + m23; +// point.b = rgbdata[3*ind+0]; +// point.g = rgbdata[3*ind+1]; +// point.r = rgbdata[3*ind+2]; +// cloud->points.push_back(point); +// } +// } +// } + + + delete[] current_occlusions; + delete[] current_overlaps; + delete[] bg_occlusions; + delete[] bg_overlaps; + } + + ArrayData3D * a3d = new ArrayData3D; + a3d->data = points; + a3d->rows = current_point; + Tree3d * tree = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + tree->buildIndex(); + + const int nr_neighbours = 20; + + for(unsigned int i = 0; i < current_point; i++){ + //if(i%1000 == 0){printf("%i/%i -> %f\n",i,current_point,float(i)/float(current_point));} + double qp [3]; + qp[0] = points[3*i+0]; + qp[1] = points[3*i+1]; + qp[2] = points[3*i+2]; + size_t ret_index [nr_neighbours]; + double out_dist_sqr [nr_neighbours]; + nanoflann::KNNResultSet resultSet(nr_neighbours); + resultSet.init(ret_index, out_dist_sqr ); + tree->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + + + } + + + if(debugg){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } + + delete[] priors; + delete[] points; + delete[] colors; + delete[] normals; + exit(0); } From 582b2cf148c34e5d7c0dfe837c241162114207ec Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 16 Aug 2016 23:58:53 +0200 Subject: [PATCH 064/255] adding to semantic map --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 2 +- .../quasimodo_brain/src/test_segment.cpp | 4 +- .../src/modelupdater/ModelUpdater.cpp | 171 +++++++++++++++--- semantic_map/CMakeLists.txt | 6 +- .../include/semantic_map/semantic_map_node.h | 87 ++++++++- semantic_map/src/semantic_map_main.cpp | 7 +- 6 files changed, 236 insertions(+), 41 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index bde87ce6..3a68fdb4 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -278,7 +278,7 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec masks.push_back(mask); } - mu->computeMovingDynamicStatic(bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions + //mu->computeMovingDynamicStatic(bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index 690a3c96..dbae5700 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -100,7 +100,7 @@ reglib::Model * load2(std::string sweep_xml){ reglib::Model * sweepmodel = 0; std::vector current_room_frames; - for (size_t i=0; i < 20 && i > internal; std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; - quasimodo_brain::segment(models[i-1],foreground,internal,external,dynamic,true); + quasimodo_brain::segment(models[i-1],foreground,internal,external,dynamic,false); // quasimodo_msgs::segment_model sm; // sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index bd9b64e2..2eacf396 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2246,13 +2246,14 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector::Ptr cloud (new pcl::PointCloud); @@ -2342,22 +2343,25 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcp, vectorbuildIndex(); - const int nr_neighbours = 20; + const int find_nr_neighbours = 10; + + std::vector > neighbour_inds; + std::vector > neighbour_weights; + neighbour_inds.resize(current_point); + neighbour_weights.resize(current_point); + std::set neighbours_list; for(unsigned int i = 0; i < current_point; i++){ - //if(i%1000 == 0){printf("%i/%i -> %f\n",i,current_point,float(i)/float(current_point));} + if(i%100000 == 0){printf("%i/%i -> %f\n",i,current_point,float(i)/float(current_point));} double qp [3]; qp[0] = points[3*i+0]; qp[1] = points[3*i+1]; qp[2] = points[3*i+2]; - size_t ret_index [nr_neighbours]; - double out_dist_sqr [nr_neighbours]; - nanoflann::KNNResultSet resultSet(nr_neighbours); + size_t ret_index [find_nr_neighbours]; + double out_dist_sqr [find_nr_neighbours]; + nanoflann::KNNResultSet resultSet(find_nr_neighbours); resultSet.init(ret_index, out_dist_sqr ); tree->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); + for(unsigned int j = 1; j < find_nr_neighbours; j++){ + int ind = ret_index[j]; + float weight = 1.00;//Make based on distance, normal and color(if same frame) + char buff [1024]; + sprintf(buff,"%i_%i",i,ind); + if(neighbours_list.count(buff) == 0){ + neighbours_list.insert(buff); + + neighbour_inds[ind].push_back(i); + neighbour_weights[ind].push_back(weight); + neighbour_inds[i].push_back(ind); + neighbour_weights[i].push_back(weight); + } + } } + for(int it = 0; it < 1000; it++){ + if(debugg){ + for(unsigned int i = 0; i < current_point; i++){ + cloud->points[i].r = estimates[3*i+0]*255.0; + cloud->points[i].g = estimates[3*i+1]*255.0; + cloud->points[i].b = estimates[3*i+2]*255.0; + } - if(debugg){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } + + double total_change = 0; + double max_change = 0; + for(unsigned int i = 0; i < current_point; i++){ + if(i%100000 == 0){printf("%i/%i -> %f\n",i,current_point,float(i)/float(current_point));} + float score0 = priors_weight[3*i+0]; + float score1 = priors_weight[3*i+1]; + float score2 = priors_weight[3*i+2]; + std::vector & neighbours = neighbour_inds[i]; + std::vector & weights = neighbour_weights[i]; + unsigned int nr_neighbours = neighbours.size(); + + + + for(unsigned int j = 0; j < nr_neighbours; j++){ + int ind = neighbours[j]; + float weight = weights[j]; + float e0 = estimates[3*ind+0]; + float e1 = estimates[3*ind+1]; + float e2 = estimates[3*ind+2]; + float minval = 0;//std::min(std::min(e0,e1),e2); + score0+=weight*(e0-minval); + score1+=weight*(e1-minval); + score2+=weight*(e2-minval); + } + + float p0 = 1-exp(-score0); + float p1 = 1-exp(-score1); + float p2 = 1-exp(-score2); + float norm = p0+p1+p2; + p0 /= norm; + p1 /= norm; + p2 /= norm; + float e0 = estimates[3*i+0]; + float e1 = estimates[3*i+1]; + float e2 = estimates[3*i+2]; + double change = fabs(e0-p0)+fabs(e1-p1)+fabs(e2-p2); + + if(true && change > max_change){ + printf("%i/%i -> ",i,current_point); + printf("prior: %f %f %f -> \n",priors_weight[3*i+0],priors_weight[3*i+1],priors_weight[3*i+2]); + + for(unsigned int j = 0; j < nr_neighbours; j++){ + int ind = neighbours[j]; + float weight = weights[j]; + float e0 = estimates[3*ind+0]; + float e1 = estimates[3*ind+1]; + float e2 = estimates[3*ind+2]; + float minval = std::min(std::min(e0,e1),e2); + printf("%f ,",weight*(e0-minval)); + printf("%f ,",weight*(e1-minval)); + printf("%f\n",weight*(e2-minval)); + } + + + printf("total: %f %f %f\n",score0,score1,score2); + + printf("\n"); + } + + total_change += change; + max_change = std::max(change,max_change); + + estimates[3*i+0] = p0; + estimates[3*i+1] = p1; + estimates[3*i+2] = p2; + + + cloud->points[i].r = std::min((change)*255.0,255.0); + cloud->points[i].g = std::min((1-change)*255.0,255.0); + cloud->points[i].b = 0; + } + printf("mean_change: %f max change: %f\n",total_change/float(current_point),max_change); viewer->removeAllPointClouds(); viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); viewer->spin(); + } + + + + delete[] estimates; + delete[] priors_weight; delete[] priors; delete[] points; delete[] colors; diff --git a/semantic_map/CMakeLists.txt b/semantic_map/CMakeLists.txt index 40002022..eb906147 100644 --- a/semantic_map/CMakeLists.txt +++ b/semantic_map/CMakeLists.txt @@ -4,7 +4,7 @@ project(semantic_map) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS mongodb_store roscpp rospy std_msgs sensor_msgs pcl_ros message_generation qt_build image_geometry cv_bridge tf_conversions strands_sweep_registration metaroom_xml_parser observation_registration_services) +find_package(catkin REQUIRED COMPONENTS quasimodo_msgs mongodb_store roscpp rospy std_msgs sensor_msgs pcl_ros message_generation qt_build image_geometry cv_bridge tf_conversions strands_sweep_registration metaroom_xml_parser observation_registration_services) find_package(Boost REQUIRED COMPONENTS system thread program_options filesystem) set(CMAKE_CXX_FLAGS "-O4 -fPIC -std=c++0x -fpermissive ${CMAKE_CXX_FLAGS}") @@ -97,8 +97,8 @@ add_executable(semantic_map_node include/semantic_map/semantic_map_node.h src/se add_executable(load_from_mongo src/load_from_mongo.cpp) add_executable(add_to_mongo src/add_to_mongo.cpp) -add_dependencies(semantic_map semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) -add_dependencies(semantic_map_node semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) +add_dependencies(semantic_map quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) +add_dependencies(semantic_map_node quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) target_link_libraries(semantic_map ${catkin_LIBRARIES} diff --git a/semantic_map/include/semantic_map/semantic_map_node.h b/semantic_map/include/semantic_map/semantic_map_node.h index 7076b4f6..a2f8f219 100644 --- a/semantic_map/include/semantic_map/semantic_map_node.h +++ b/semantic_map/include/semantic_map/semantic_map_node.h @@ -45,6 +45,16 @@ #include "reg_transforms.h" #include "room_utilities.h" +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" +#include "quasimodo_msgs/segment_model.h" +#include "quasimodo_msgs/metaroom_pair.h" +#include + template @@ -76,6 +86,7 @@ class SemanticMapNode { ros::Publisher m_PublisherDynamicClusters; ros::Publisher m_PublisherStatus; ros::ServiceServer m_ClearMetaroomServiceServer; + ros::ServiceClient m_segmentation_client; private: @@ -83,7 +94,7 @@ class SemanticMapNode { SemanticMapSummaryParser m_SummaryParser; bool m_bSaveIntermediateData; std::vector > > m_vLoadedMetarooms; - mongodb_store::MessageStoreProxy m_messageStore; + //mongodb_store::MessageStoreProxy m_messageStore; bool m_bLogToDB; std::map > > m_WaypointToMetaroomMap; std::map > m_WaypointToRoomMap; @@ -92,15 +103,18 @@ class SemanticMapNode { bool m_bNewestClusters; bool m_bUseNDTRegistration; int m_MinObjectSize; + int segmentationtype; }; template -SemanticMapNode::SemanticMapNode(ros::NodeHandle nh) : m_messageStore(nh) +SemanticMapNode::SemanticMapNode(ros::NodeHandle nh)// : m_messageStore(nh) { + segmentationtype = 1; ROS_INFO_STREAM("Semantic map node initialized"); m_NodeHandle = nh; + m_segmentation_client = nh.serviceClient("segment_metaroom"); m_SubscriberRoomObservation = m_NodeHandle.subscribe("/local_metric_map/room_observations",1, &SemanticMapNode::roomObservationCallback,this); m_PublisherMetaroom = m_NodeHandle.advertise("/local_metric_map/metaroom", 1, true); @@ -119,6 +133,7 @@ SemanticMapNode::SemanticMapNode(ros::NodeHandle nh) : m_messageStore m_NodeHandle.param("log_to_db",m_bLogToDB,false); if (m_bLogToDB) { + //m_messageStore = mongodb_store::MessageStoreProxy(nh); ROS_INFO_STREAM("Logging dynamic clusters to the database."); } else { ROS_INFO_STREAM("NOT logging dynamic clusters to the database."); @@ -281,6 +296,69 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam MetaRoomXMLParser meta_parser; meta_parser.saveMetaRoomAsXML(*metaroom); } + if(segmentationtype == 1){ + std::string previousObservationXml; + passwd* pw = getpwuid(getuid()); + std::string dataPath(pw->pw_dir); + dataPath+="/.semanticMap"; + + std::vector matchingObservations = semantic_map_load_utilties::getSweepXmlsForTopologicalWaypoint(dataPath, aRoom.getRoomStringId()); + if (matchingObservations.size() == 0) // no observations -> first observation + { + ROS_INFO_STREAM("No observations for this waypoint saved on the disk "+aRoom.getRoomStringId()+" cannot compare with previous observation."); + std_msgs::String msg; + msg.data = "finished_processing_observation"; + m_PublisherStatus.publish(msg); + return; + } + if (matchingObservations.size() == 1) // first observation at this waypoint + { + ROS_INFO_STREAM("No dynamic clusters."); + std_msgs::String msg; + msg.data = "finished_processing_observation"; + m_PublisherStatus.publish(msg); + return; + } + + int stopind = 0; + + for(unsigned int i = 0; i < matchingObservations.size(); i++){ + if(matchingObservations[i].compare(xml_file_name) == 0){ + break; + }else{ + stopind = i;//latest = matchingObservations[i]; + } + } + + if(stopind == 0) // first observation at this waypoint + { + ROS_INFO_STREAM("No dynamic clusters."); + std_msgs::String msg; + msg.data = "finished_processing_observation"; + m_PublisherStatus.publish(msg); + return; + } + previousObservationXml = matchingObservations[stopind]; + printf("prev: %s\n",previousObservationXml.c_str()); + // multiple observations -> find the previous one +// reverse(matchingObservations.begin(), matchingObservations.end()); +// latest = matchingObservations[0]; +// if (latest != xml_file_name) +// { +// ROS_WARN_STREAM("The xml file for the latest observations "+latest+" is different from the one provided "+xml_file_name+" Aborting"); +// } +// previousObservationXml = matchingObservations[1]; + + quasimodo_msgs::metaroom_pair sm; + sm.request.background = previousObservationXml; + sm.request.foreground = xml_file_name; + + if (m_segmentation_client.call(sm)){ + //int model_id = mff.response.model_id; + }else{ROS_ERROR("Failed to call service segment_model");} + +exit(0); + }else{ CloudPtr difference(new Cloud()); // this stores the difference between a) current obs and previous obs or b) current obs and metaroom (depending on the node parameters) if (m_bNewestClusters){ @@ -495,7 +573,7 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam m_WaypointToDynamicClusterMap[aRoom.getRoomStringId()] = dynamicClusters; ROS_INFO_STREAM("Updated map with new dynamic clusters for waypoint "<::processRoomObservation(std::string xml_file_nam } } } - +*/ +} std_msgs::String msg; msg.data = "finished_processing_observation"; m_PublisherStatus.publish(msg); diff --git a/semantic_map/src/semantic_map_main.cpp b/semantic_map/src/semantic_map_main.cpp index 2a3dd1fd..352a2cdb 100644 --- a/semantic_map/src/semantic_map_main.cpp +++ b/semantic_map/src/semantic_map_main.cpp @@ -2,14 +2,17 @@ int main(int argc, char **argv) { + printf("test\n"); // Set up ROS. ros::init(argc, argv, "Semantic_map_node"); ros::NodeHandle n; ros::NodeHandle aRosNode("~"); - + printf("test\n"); SemanticMapNode aSemanticMapNode(aRosNode); - + aSemanticMapNode.processRoomObservation(argv[1]); + exit(0); + printf("test\n"); ros::Rate loop_rate(10); while (ros::ok()) { From ed407e379328e183ef1e937560b2fccb16b5cad8 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 17 Aug 2016 17:25:22 +0200 Subject: [PATCH 065/255] working on stability and integration of segmentation --- .../src/segment_room_pairs.cpp | 15 +- .../src/segmentationserver.cpp | 145 ++---------------- .../src/test_segment_room_pairs.cpp | 11 +- .../src/modelupdater/ModelUpdater.cpp | 6 +- .../quasimodo_msgs/srv/metaroom_pair.srv | 3 +- .../quasimodo_msgs/srv/segment_model.srv | 2 +- .../include/semantic_map/semantic_map_node.h | 65 +++++--- semantic_map/src/semantic_map_main.cpp | 3 +- 8 files changed, 80 insertions(+), 170 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp index 7af6ddd1..9595583f 100644 --- a/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp +++ b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp @@ -79,15 +79,24 @@ bool segment_metaroom(quasimodo_msgs::metaroom_pair::Request & req, quasimodo_m sm.request.backgroundmodel = quasimodo_brain::getModelMSG(bg_model); sm.request.models.push_back(quasimodo_brain::getModelMSG(fg_model)); + bool status; + if (segmentation_client.call(sm)){ - //int model_id = mff.response.model_id; - }else{ROS_ERROR("Failed to call service segment_model");} + if(sm.response.dynamicmasks.size() > 0){ + res.dynamicmasks = sm.response.dynamicmasks.front().images; + res.movingmasks = sm.response.movingmasks.front().images; + } + status = true; + }else{ + ROS_ERROR("Failed to call service segment_model"); + status = false; + } bg_model->fullDelete(); delete bg_model; fg_model->fullDelete(); delete fg_model; - return true; + return status; } int main(int argc, char** argv){ diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index fa8e33fa..5057cacf 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -38,11 +38,9 @@ boost::shared_ptr viewer; //Update background //How? - -//std::vector - bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs::segment_model::Response & res){ printf("segment_model\n"); + std::vector< reglib::Model * > models; for(unsigned int i = 0; i < req.models.size(); i++){ models.push_back(quasimodo_brain::getModelFromMSG(req.models[i])); @@ -53,128 +51,6 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs std::vector< std::vector< cv::Mat > > internal; std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; -/* - reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.25); - massregmod->timeout = 1200; - massregmod->viewer = viewer; - massregmod->visualizationLvl = 0; - massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massregmod->nomask = true; - massregmod->stopval = 0.0001; - - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; - - - if(models.size() > 0 && bg->frames.size() > 0){ - vector cpmod; - - bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); - cpmod.push_back(Eigen::Matrix4d::Identity());//,models.front()->relativeposes.front().inverse() * bg->relativeposes.front()); - massregmod->addModel(bg); - - for(int j = 0; j < models.size(); j++){ - models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - cpmod.push_back(bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); - massregmod->addModel(models[j]); - } - - reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); - - for(int j = 0; j < models.size(); j++){ - Eigen::Matrix4d change = mfrmod.poses[j+1] * cpmod[j+1].inverse(); - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; - } - } - }else if(models.size() > 1){ - vector cpmod; - - for(int j = 0; j < models.size(); j++){ - models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); - massregmod->addModel(models[j]); - } - - reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); - for(int j = 0; j < models.size(); j++){ - Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; - } - } - } - - delete massregmod; - - - vector bgcp; - vector bgcf; - vector bgmask; - - for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ - bgcp.push_back(bg->relativeposes[k]); - bgcf.push_back(bg->frames[k]); - bgmask.push_back(bg->modelmasks[k]->getMask()); - } - - - - - for(int j = 0; j < models.size(); j++){ - reglib::Model * model = models[j]; - - vector masks; - for(unsigned int i = 0; i < model->frames.size(); i++){ - reglib::RGBDFrame * frame = model->frames[i]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} - masks.push_back(mask); - } - - std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions - std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,false);//Determine external occlusions - std::vector dynamic_masks; - for(unsigned int i = 0; i < model->frames.size(); i++){ - reglib::RGBDFrame * frame = model->frames[i]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} - - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); - unsigned char * externalmaskdata = (unsigned char *)(external_masks[i].data); - for(unsigned int k = 0; k < cam->height * cam->width;k++){ - if(externalmaskdata[k] == 0 && internalmaskdata[k] != 0 ){ - maskdata[k] = 255; - }else{ - maskdata[k] = 0; - } - } - - dynamic_masks.push_back(mask); - -// cv::imshow( "rgb", frame->rgb ); -// cv::imshow( "internal_masks", internal_masks[i] ); -// cv::imshow( "externalmask", external_masks[i] ); -// cv::imshow( "dynamic_mask", dynamic_masks[i] ); -// cv::waitKey(0); - } - - internal.push_back(internal_masks); - external.push_back(external_masks); - dynamic.push_back(dynamic_masks); - } -*/ quasimodo_brain::segment(bg,models,internal,external,dynamic); @@ -192,13 +68,6 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs reglib::Camera * camera = frame->camera; -// cv::imshow( "rgb", frame->rgb ); -// cv::imshow( "internal_masks", internal_masks[j] ); -// cv::imshow( "externalmask", external_masks[j] ); -// cv::imshow( "dynamic_mask", dynamic_masks[j] ); -// cv::waitKey(100); - - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); @@ -257,9 +126,8 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs res.backgroundmodel = req.backgroundmodel; - res.dynamicmasks.resize(models.size()); - res.internalmasks.resize(models.size()); + res.movingmasks.resize(models.size()); for(unsigned int i = 0; i < models.size(); i++){ for(unsigned int j = 0; j < models[i]->frames.size(); j++){ cv_bridge::CvImage dynamicmaskBridgeImage; @@ -270,15 +138,22 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs cv_bridge::CvImage internalmaskBridgeImage; internalmaskBridgeImage.image = internal[i][j]; internalmaskBridgeImage.encoding = "mono8"; - res.internalmasks[i].images.push_back( *(internalmaskBridgeImage.toImageMsg()) ); + res.movingmasks[i].images.push_back( *(internalmaskBridgeImage.toImageMsg()) ); } res.models.push_back(quasimodo_brain::getModelMSG(models[i])); + } + + + for(unsigned int i = 0; i < models.size(); i++){ models[i]->fullDelete(); delete models[i]; } models.clear(); + bg->fullDelete(); + delete bg; + return true; } diff --git a/quasimodo/quasimodo_brain/src/test_segment_room_pairs.cpp b/quasimodo/quasimodo_brain/src/test_segment_room_pairs.cpp index 9a63d813..d335aa96 100644 --- a/quasimodo/quasimodo_brain/src/test_segment_room_pairs.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment_room_pairs.cpp @@ -68,9 +68,7 @@ using namespace std; int main(int argc, char** argv){ - ros::init(argc, argv, "test_segmentationserver_metaroom"); - ros::NodeHandle n; - ros::ServiceClient segmentation_client = n.serviceClient("segment_metaroom"); + vector room_xmls; for(int ar = 1; ar < argc; ar++){ @@ -84,13 +82,16 @@ int main(int argc, char** argv){ } } + ros::init(argc, argv, "test_segmentationserver_metaroom"); + ros::NodeHandle n; + ros::ServiceClient segmentation_client = n.serviceClient("segment_metaroom"); quasimodo_msgs::metaroom_pair sm; sm.request.background = std::string(room_xmls[0]); sm.request.foreground = std::string(room_xmls[1]); - +for(int i = 0; i < 150; i++){ if (segmentation_client.call(sm)){ //int model_id = mff.response.model_id; }else{ROS_ERROR("Failed to call service segment_model");} - +} return 0; } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 2eacf396..cd97f333 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -847,9 +847,9 @@ void ModelUpdater::addSuperPoints(vector & spvec, Matrix4d p, RGBDFr float pny = m10*dst_nx + m11*dst_ny + m12*dst_nz; float pnz = m20*dst_nx + m21*dst_ny + m22*dst_nz; - float pb = rgbdata[3*ind+0]; - float pg = rgbdata[3*ind+1]; - float pr = rgbdata[3*ind+2]; + float pb = rgbdata[3*dst_ind+0]; + float pg = rgbdata[3*dst_ind+1]; + float pr = rgbdata[3*dst_ind+2]; Vector3f pxyz (px ,py ,pz ); Vector3f pnxyz (pnx,pny,pnz); diff --git a/quasimodo/quasimodo_msgs/srv/metaroom_pair.srv b/quasimodo/quasimodo_msgs/srv/metaroom_pair.srv index fc2665e3..9f3907d7 100644 --- a/quasimodo/quasimodo_msgs/srv/metaroom_pair.srv +++ b/quasimodo/quasimodo_msgs/srv/metaroom_pair.srv @@ -1,4 +1,5 @@ string background string foreground --- -sensor_msgs/Image[] segmentation +sensor_msgs/Image[] dynamicmasks +sensor_msgs/Image[] movingmasks diff --git a/quasimodo/quasimodo_msgs/srv/segment_model.srv b/quasimodo/quasimodo_msgs/srv/segment_model.srv index fd17c862..818264a3 100644 --- a/quasimodo/quasimodo_msgs/srv/segment_model.srv +++ b/quasimodo/quasimodo_msgs/srv/segment_model.srv @@ -7,4 +7,4 @@ uint64 models_refine model backgroundmodel model[] models image_array[] dynamicmasks -image_array[] internalmasks +image_array[] movingmasks diff --git a/semantic_map/include/semantic_map/semantic_map_node.h b/semantic_map/include/semantic_map/semantic_map_node.h index a2f8f219..6b6ca6ee 100644 --- a/semantic_map/include/semantic_map/semantic_map_node.h +++ b/semantic_map/include/semantic_map/semantic_map_node.h @@ -296,6 +296,8 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam MetaRoomXMLParser meta_parser; meta_parser.saveMetaRoomAsXML(*metaroom); } + + CloudPtr difference(new Cloud()); if(segmentationtype == 1){ std::string previousObservationXml; passwd* pw = getpwuid(getuid()); @@ -340,27 +342,48 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam } previousObservationXml = matchingObservations[stopind]; printf("prev: %s\n",previousObservationXml.c_str()); - // multiple observations -> find the previous one -// reverse(matchingObservations.begin(), matchingObservations.end()); -// latest = matchingObservations[0]; -// if (latest != xml_file_name) -// { -// ROS_WARN_STREAM("The xml file for the latest observations "+latest+" is different from the one provided "+xml_file_name+" Aborting"); -// } -// previousObservationXml = matchingObservations[1]; - - quasimodo_msgs::metaroom_pair sm; - sm.request.background = previousObservationXml; - sm.request.foreground = xml_file_name; - - if (m_segmentation_client.call(sm)){ - //int model_id = mff.response.model_id; - }else{ROS_ERROR("Failed to call service segment_model");} - -exit(0); + + quasimodo_msgs::metaroom_pair sm; + sm.request.background = previousObservationXml; + sm.request.foreground = xml_file_name; + + if (m_segmentation_client.call(sm)){ + CloudPtr dynamiccloud(new Cloud()); + std::vector dynamicmasks; + for(unsigned int i = 0; i < sm.response.dynamicmasks.size(); i++){ + cv_bridge::CvImagePtr img_ptr; + try{ img_ptr = cv_bridge::toCvCopy(sm.response.dynamicmasks[i], sensor_msgs::image_encodings::MONO8);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat mask = img_ptr->image; + dynamicmasks.push_back(mask); + + CloudPtr currentcloud = aRoom.getIntermediateClouds()[i]; + CloudPtr currentcloudTMP(new Cloud()); + aRoom.getIntermediateCloudTransformsRegistered()[i]; + //pcl::transformPointCloud (*currentcloud, *currentcloudTMP, aRoom.m_vIntermediateRoomCloudTransformsRegistered[i]); + + pcl_ros::transformPointCloud(*currentcloud, *currentcloudTMP,aRoom.getIntermediateCloudTransformsRegistered()[i]); + unsigned char * maskdata = mask.data; + for(unsigned int j = 0; j < currentcloudTMP->points.size(); j++){ + if(maskdata[j] > 0){ + difference->points.push_back(currentcloudTMP->points[j]); + } + } + } + +// std::vector vClusters = MetaRoom::clusterPointCloud(dynamiccloud,0.03,100,1000000); +// ROS_INFO_STREAM("Clustered differences. "< register with the previous observation // look for the previous observation in ~/.semanticMap/ @@ -507,7 +530,7 @@ exit(0); ROS_INFO_STREAM("Comparison cloud "<points.size()<<" room cloud "<points.size()); } - +} ROS_INFO_STREAM("Raw difference "<points.size()); @@ -592,7 +615,7 @@ exit(0); } } */ -} + std_msgs::String msg; msg.data = "finished_processing_observation"; m_PublisherStatus.publish(msg); diff --git a/semantic_map/src/semantic_map_main.cpp b/semantic_map/src/semantic_map_main.cpp index 352a2cdb..2014ff7c 100644 --- a/semantic_map/src/semantic_map_main.cpp +++ b/semantic_map/src/semantic_map_main.cpp @@ -9,7 +9,8 @@ int main(int argc, char **argv) ros::NodeHandle aRosNode("~"); printf("test\n"); - SemanticMapNode aSemanticMapNode(aRosNode); + //SemanticMapNode aSemanticMapNode(aRosNode); + SemanticMapNode aSemanticMapNode(n); aSemanticMapNode.processRoomObservation(argv[1]); exit(0); printf("test\n"); From 507b349996e978eb4d73626d9c2dfcc351088b69 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 18 Aug 2016 17:14:29 +0200 Subject: [PATCH 066/255] integration with metarooms --- .../metaroom_xml_parser/load_utilities.hpp | 2 +- .../src/modelupdater/ModelUpdater.cpp | 2 +- semantic_map/CMakeLists.txt | 12 +- .../include/semantic_map/room_xml_parser.hpp | 6 + .../include/semantic_map/semantic_map_node.h | 52 ++- semantic_map/src/semantic_map_recompute.cpp | 377 ++++++++++++++++++ 6 files changed, 426 insertions(+), 25 deletions(-) create mode 100644 semantic_map/src/semantic_map_recompute.cpp diff --git a/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp b/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp index e637365c..e07d1061 100644 --- a/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp +++ b/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp @@ -404,7 +404,7 @@ std::vector> loadAllDynamicObjectsFromSingleSweep(s if (verbose){ std::cout<<"Found "< > getImageProbs(reglib::RGBDFrame * frame, int DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); func->zeromean = true; - func->maxp = 0.99; + func->maxp = 0.999; func->startreg = 0.0; func->debugg_print = false; func->bidir = true; diff --git a/semantic_map/CMakeLists.txt b/semantic_map/CMakeLists.txt index eb906147..01f85ccb 100644 --- a/semantic_map/CMakeLists.txt +++ b/semantic_map/CMakeLists.txt @@ -94,11 +94,13 @@ set(SRCS add_library(semantic_map ${HDRS} ${HDRS_IMPL} ${SRCS}) add_executable(semantic_map_node include/semantic_map/semantic_map_node.h src/semantic_map_node.cpp src/semantic_map_main.cpp) +add_executable(semantic_map_recompute include/semantic_map/semantic_map_node.h src/semantic_map_node.cpp src/semantic_map_recompute.cpp) add_executable(load_from_mongo src/load_from_mongo.cpp) add_executable(add_to_mongo src/add_to_mongo.cpp) add_dependencies(semantic_map quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) add_dependencies(semantic_map_node quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) +add_dependencies(semantic_map_recompute quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) target_link_libraries(semantic_map ${catkin_LIBRARIES} @@ -115,6 +117,14 @@ add_dependencies(semantic_map_node quasimodo_msgs_generate_messages_cpp semantic semantic_map ) + target_link_libraries(semantic_map_recompute + ${catkin_LIBRARIES} + ${PCL_LIBRARIES} + ${QT_LIBRARIES} + ${Boost_LIBRARIES} + semantic_map + ) + target_link_libraries(load_from_mongo ${catkin_LIBRARIES} ${PCL_LIBRARIES} @@ -133,7 +143,7 @@ add_dependencies(semantic_map_node quasimodo_msgs_generate_messages_cpp semantic ############################# INSTALL TARGETS -install(TARGETS semantic_map semantic_map_node load_from_mongo +install(TARGETS semantic_map semantic_map_node semantic_map_recompute load_from_mongo ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/semantic_map/include/semantic_map/room_xml_parser.hpp b/semantic_map/include/semantic_map/room_xml_parser.hpp index fb699a90..e35aaf71 100644 --- a/semantic_map/include/semantic_map/room_xml_parser.hpp +++ b/semantic_map/include/semantic_map/room_xml_parser.hpp @@ -55,6 +55,7 @@ bool SemanticRoomXMLParser::setRootFolderFromRoomXml(std::string room template std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoom& aRoom, std::string xmlFile, bool verbose ) { + printf("saveRoomAsXML\n"); // create folder structure QString roomLogName(aRoom.getRoomLogName().c_str()); int index = roomLogName.indexOf('_'); @@ -202,8 +203,12 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoompoints.size()) // only save the cloud file if it's been loaded { + printf("there are dynamic clusters found\n"); // if (!dynamicClustersFile.exists()) // { pcl::io::savePCDFileBinary(dynamicClustersFilename.toStdString(), *aRoom.getDynamicClustersCloud()); @@ -218,6 +223,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteAttribute("filename",dynamicClustersFilenameLocal); xmlWriter->writeEndElement(); } else { + printf("no dynamic clusters found\n"); if (dynamicClustersFile.exists()) { xmlWriter->writeStartElement("RoomDynamicClusters"); diff --git a/semantic_map/include/semantic_map/semantic_map_node.h b/semantic_map/include/semantic_map/semantic_map_node.h index 6b6ca6ee..07b8b6a5 100644 --- a/semantic_map/include/semantic_map/semantic_map_node.h +++ b/semantic_map/include/semantic_map/semantic_map_node.h @@ -165,7 +165,7 @@ SemanticMapNode::SemanticMapNode(ros::NodeHandle nh)// : m_messageSto // } - m_NodeHandle.param("min_object_size",m_MinObjectSize,500); + m_NodeHandle.param("min_object_size",m_MinObjectSize,500); ROS_INFO_STREAM("Min object size set to"<::processRoomObservation(std::string xml_file_nam } ROS_INFO_STREAM("Raw difference "<points.size()); - +/* if (difference->points.size() == 0) { // -> no dynamic clusters can be computed @@ -543,17 +543,18 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam m_PublisherStatus.publish(msg); return; } - +*/ std::vector vClusters = MetaRoom::clusterPointCloud(difference,0.03,m_MinObjectSize,100000); ROS_INFO_STREAM("Clustered differences. "<::filterClustersBasedOnDistance(aRoom.getIntermediateCloudTransforms()[0].getOrigin(), vClusters,4.0); - ROS_INFO_STREAM(vClusters.size()<<" different clusters after max distance filtering."); + MetaRoom::filterClustersBasedOnDistance(aRoom.getIntermediateCloudTransforms()[0].getOrigin(), vClusters,4.0); + ROS_INFO_STREAM(vClusters.size()<<" different clusters after max distance filtering."); ROS_INFO_STREAM("Clustered differences. "< seg; @@ -575,6 +576,12 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam *dynamicClusters += *vClusters[i]; } } +*/ + + for (size_t i=0; i::processRoomObservation(std::string xml_file_nam m_WaypointToDynamicClusterMap[aRoom.getRoomStringId()] = dynamicClusters; ROS_INFO_STREAM("Updated map with new dynamic clusters for waypoint "< > results; - if(m_messageStore.queryNamed(databaseName.toStdString(), results)) { +// if (m_bLogToDB) +// { +// QString databaseName = QString(aRoom.getRoomLogName().c_str()) + QString("/room_")+ QString::number(aRoom.getRoomRunNumber()) +QString("/dynamic_clusters"); +// std::string id(m_messageStore.insertNamed(databaseName.toStdString(), msg_clusters)); +// ROS_INFO_STREAM("Dynamic clusters \""< > results; +// if(m_messageStore.queryNamed(databaseName.toStdString(), results)) { + +// BOOST_FOREACH( boost::shared_ptr p, results) +// { +// CloudPtr databaseCloud(new Cloud()); +// pcl::fromROSMsg(*p,*databaseCloud); +// ROS_INFO_STREAM("Got pointcloud by name. No points: " << databaseCloud->points.size()); +// } +// } +// } - BOOST_FOREACH( boost::shared_ptr p, results) - { - CloudPtr databaseCloud(new Cloud()); - pcl::fromROSMsg(*p,*databaseCloud); - ROS_INFO_STREAM("Got pointcloud by name. No points: " << databaseCloud->points.size()); - } - } - } -*/ std_msgs::String msg; msg.data = "finished_processing_observation"; m_PublisherStatus.publish(msg); + return; } diff --git a/semantic_map/src/semantic_map_recompute.cpp b/semantic_map/src/semantic_map_recompute.cpp new file mode 100644 index 00000000..ae3ee122 --- /dev/null +++ b/semantic_map/src/semantic_map_recompute.cpp @@ -0,0 +1,377 @@ +#include "semantic_map/semantic_map_node.h" + +#include "eigen_conversions/eigen_msg.h" +#include "tf_conversions/tf_eigen.h" + +typedef pcl::PointXYZRGB PointType; +typedef pcl::PointCloud Cloud; +typedef typename Cloud::Ptr CloudPtr; +typedef pcl::search::KdTree Tree; +typedef semantic_map_load_utilties::DynamicObjectData ObjectData; + +using namespace std; +using namespace semantic_map_load_utilties; + +std::string getPrevXML(std::string xml_file_name){ + std::cout<<"File name "< parser; + SemanticRoom aRoom = SemanticRoomXMLParser::loadRoomFromXML(xml_file_name,true); + // aRoom.resetRoomTransform(); + // CloudPtr room_complete_cloud = aRoom.getCompleteRoomCloud(); + + // // update summary xml + // m_SummaryParser.createSummaryXML(); + + // // update list of rooms & metarooms + // m_SummaryParser.refresh(); + + ROS_INFO_STREAM("Summary XML created. Looking for metaroom for room with id: "< > metaroom; +// bool found = false; + +// std::string matchingMetaroomXML = ""; +// // first check if the matching metaroom has already been loaded +// for (size_t i=0; im_sMetaroomStringId) +// { +// metaroom = m_vLoadedMetarooms[i]; +// found = true; +// break; +// } +// } else { +// // if not set, compare by centroid +// double centroidDistance = pcl::distances::l2(m_vLoadedMetarooms[i]->getCentroid(),aRoom.getCentroid()); +// if (! (centroidDistance < ROOM_CENTROID_DISTANCE) ) +// { +// continue; +// } else { +// ROS_INFO_STREAM("Matching metaroom already loaded."); +// metaroom = m_vLoadedMetarooms[i]; +// found = true; +// break; +// } +// } +// } + +// // if not loaded already, look through already saved metarooms +// if (!found) +// { +// std::vector allMetarooms = m_SummaryParser.getMetaRooms(); +// ROS_INFO_STREAM("Loaded "< >(new MetaRoom()); +// metaroom->m_sMetaroomStringId = aRoom.getRoomStringId(); // set waypoint, to figure out where to save + +// } else { +// ROS_INFO_STREAM("Matching metaroom found. XML file: "<::loadMetaRoomFromXML(matchingMetaroomXML,true); +// found = true; +// } +// } else { +// MetaRoomXMLParser meta_parser; +// matchingMetaroomXML= meta_parser.findMetaRoomLocation(metaroom.get()).toStdString(); +// matchingMetaroomXML+="/metaroom.xml"; +// } + +// if (!found) +// { +// ROS_INFO_STREAM("Initializing metaroom."); +// metaroom->setUpdateMetaroom(m_bUpdateMetaroom); +// metaroom->setSaveIntermediateSteps(m_bSaveIntermediateData); +// // also update the metaroom here +// auto updateIteration = metaroom->updateMetaRoom(aRoom,"",true); +// // save metaroom +// ROS_INFO_STREAM("Saving metaroom."); +// MetaRoomXMLParser meta_parser; +// meta_parser.saveMetaRoomAsXML(*metaroom); +// } + +// CloudPtr difference(new Cloud()); +// if(segmentationtype == 1){ +// std::string previousObservationXml; +// passwd* pw = getpwuid(getuid()); +// std::string dataPath(pw->pw_dir); +// dataPath+="/.semanticMap"; + +// std::vector matchingObservations = semantic_map_load_utilties::getSweepXmlsForTopologicalWaypoint(dataPath, aRoom.getRoomStringId()); +// if (matchingObservations.size() == 0) // no observations -> first observation +// { +// ROS_INFO_STREAM("No observations for this waypoint saved on the disk "+aRoom.getRoomStringId()+" cannot compare with previous observation."); +// std_msgs::String msg; +// msg.data = "finished_processing_observation"; +// m_PublisherStatus.publish(msg); +// return; +// } +// if (matchingObservations.size() == 1) // first observation at this waypoint +// { +// ROS_INFO_STREAM("No dynamic clusters."); +// std_msgs::String msg; +// msg.data = "finished_processing_observation"; +// m_PublisherStatus.publish(msg); +// return; +// } + +// int stopind = 0; + +// for(unsigned int i = 0; i < matchingObservations.size(); i++){ +// if(matchingObservations[i].compare(xml_file_name) == 0){ +// break; +// }else{ +// stopind = i;//latest = matchingObservations[i]; +// } +// } + +// if(stopind == 0) // first observation at this waypoint +// { +// ROS_INFO_STREAM("No dynamic clusters."); +// std_msgs::String msg; +// msg.data = "finished_processing_observation"; +// m_PublisherStatus.publish(msg); +// return; +// } +// previousObservationXml = matchingObservations[stopind]; +// printf("prev: %s\n",previousObservationXml.c_str()); + +// quasimodo_msgs::metaroom_pair sm; +// sm.request.background = previousObservationXml; +// sm.request.foreground = xml_file_name; + +// if (m_segmentation_client.call(sm)){ +// CloudPtr dynamiccloud(new Cloud()); +// std::vector dynamicmasks; +// for(unsigned int i = 0; i < sm.response.dynamicmasks.size(); i++){ +// cv_bridge::CvImagePtr img_ptr; +// try{ img_ptr = cv_bridge::toCvCopy(sm.response.dynamicmasks[i], sensor_msgs::image_encodings::MONO8);} +// catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} +// cv::Mat mask = img_ptr->image; +// dynamicmasks.push_back(mask); + +// CloudPtr currentcloud = aRoom.getIntermediateClouds()[i]; +// CloudPtr currentcloudTMP(new Cloud()); +// aRoom.getIntermediateCloudTransformsRegistered()[i]; +// //pcl::transformPointCloud (*currentcloud, *currentcloudTMP, aRoom.m_vIntermediateRoomCloudTransformsRegistered[i]); + +// pcl_ros::transformPointCloud(*currentcloud, *currentcloudTMP,aRoom.getIntermediateCloudTransformsRegistered()[i]); +// unsigned char * maskdata = mask.data; +// for(unsigned int j = 0; j < currentcloudTMP->points.size(); j++){ +// if(maskdata[j] > 0){ +// difference->points.push_back(currentcloudTMP->points[j]); +// } +// } +// } + +// // std::vector vClusters = MetaRoom::clusterPointCloud(dynamiccloud,0.03,100,1000000); +// // ROS_INFO_STREAM("Clustered differences. "< viewrgbs; + std::vector viewdepths; + std::vector viewtfs; + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); + for (auto objectFile : objectFiles){ + auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); + for (unsigned int i=0; iheight,cloud->width,CV_8UC3); + unsigned char * rgbdata = (unsigned char *)rgb.data; + + cv::Mat depth; + depth.create(cloud->height,cloud->width,CV_16UC1); + unsigned short * depthdata = (unsigned short *)depth.data; + + unsigned int nr_data = cloud->height * cloud->width; + for(unsigned int j = 0; j < nr_data; j++){ + PointType p = cloud->points[j]; + rgbdata[3*j+0] = p.b; + rgbdata[3*j+1] = p.g; + rgbdata[3*j+2] = p.r; + depthdata[j] = short(5000.0 * p.z); + } + + viewrgbs.push_back(rgb); + viewdepths.push_back(depth); + viewtfs.push_back(object.vAdditionalViewsTransforms[i]); + //viewcams.push_back(roomData.vIntermediateRoomCloudCamParams.front()); + + cv::namedWindow("rgbimage", cv::WINDOW_AUTOSIZE); + cv::imshow( "rgbimage", rgb); + cv::namedWindow("depthimage", cv::WINDOW_AUTOSIZE); + cv::imshow( "depthimage", depth); + cv::waitKey(30); + + } + } + + QFile file((sweep_folder+"additionalViews.xml").c_str()); + if (file.exists()){file.remove();} + + + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)) + { + std::cerr<<"Could not open file "<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("AdditionalViews"); + + for(unsigned int i = 0; i < viewrgbs.size(); i++){ + char buf [1024]; + sprintf(buf,"%s/RGB%10.10i.png",sweep_folder.c_str(),i); + cv::imwrite(buf, viewrgbs[i] ); + sprintf(buf,"%s/DEPTH%10.10i.png",sweep_folder.c_str(),i); + cv::imwrite(buf, viewdepths[i] ); + + xmlWriter->writeStartElement("AdditionalViewTransform"); + + geometry_msgs::TransformStamped msg; + tf::transformStampedTFToMsg(viewtfs[i], msg); + + xmlWriter->writeStartElement("Stamp"); + xmlWriter->writeStartElement("sec"); + xmlWriter->writeCharacters(QString::number(msg.header.stamp.sec)); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("nsec"); + xmlWriter->writeCharacters(QString::number(msg.header.stamp.nsec)); + xmlWriter->writeEndElement(); + xmlWriter->writeEndElement(); // Stamp + + xmlWriter->writeStartElement("FrameId"); + xmlWriter->writeCharacters(QString(msg.header.frame_id.c_str())); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("ChildFrameId"); + xmlWriter->writeCharacters(QString(msg.child_frame_id.c_str())); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("Transform"); + xmlWriter->writeStartElement("Translation"); + xmlWriter->writeStartElement("x"); + xmlWriter->writeCharacters(QString::number(msg.transform.translation.x)); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("y"); + xmlWriter->writeCharacters(QString::number(msg.transform.translation.y)); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("z"); + xmlWriter->writeCharacters(QString::number(msg.transform.translation.z)); + xmlWriter->writeEndElement(); + xmlWriter->writeEndElement(); // Translation + xmlWriter->writeStartElement("Rotation"); + xmlWriter->writeStartElement("w"); + xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w)); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("x"); + xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x)); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("y"); + xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y)); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("z"); + xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z)); + xmlWriter->writeEndElement(); + xmlWriter->writeEndElement(); // Rotation + xmlWriter->writeEndElement(); // Transform + xmlWriter->writeEndElement(); // RoomIntermediateCloudTransform + } + xmlWriter->writeEndElement(); // Semantic Room + xmlWriter->writeEndDocument(); + delete xmlWriter; +} + +void segmentWithAdditionalViewsXml(std::string sweep_xml,std::string prev){ + printf("segmentWithAdditionalViewsXml\n"); +// std::string prev = getPrevXML(sweep_xml); +} + + +int main(int argc, char **argv) +{ + for(int ar = 1; ar < argc; ar++){ + std::string overall_folder = std::string(argv[ar]); + printf("overall folder: %s\n",overall_folder.c_str()); + + std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + for (int i = 0; i < sweep_xmls.size(); i++){//auto sweep_xml : sweep_xmls) { + printf("current xml: %s\n",sweep_xmls[i].c_str()); + fixXml(sweep_xmls[i]); + if(i > 1){ + segmentWithAdditionalViewsXml(sweep_xmls[i-1],sweep_xmls[i]); + } + } + } + + printf("test\n"); + exit(0); + // Set up ROS. + ros::init(argc, argv, "Semantic_map_node"); + ros::NodeHandle n; + + ros::NodeHandle aRosNode("~"); + printf("test\n"); + //SemanticMapNode aSemanticMapNode(aRosNode); + SemanticMapNode aSemanticMapNode(n); + aSemanticMapNode.processRoomObservation(argv[1]); + exit(0); + printf("test\n"); + ros::Rate loop_rate(10); + while (ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + } +} From 76e3129b6f93526bb4bed341c9fd2dbb11bbe77d Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 18 Aug 2016 22:22:54 +0200 Subject: [PATCH 067/255] working on additional view registration and segmentation --- semantic_map/src/semantic_map_recompute.cpp | 207 ++++++++++++++++---- 1 file changed, 174 insertions(+), 33 deletions(-) diff --git a/semantic_map/src/semantic_map_recompute.cpp b/semantic_map/src/semantic_map_recompute.cpp index ae3ee122..25e934cc 100644 --- a/semantic_map/src/semantic_map_recompute.cpp +++ b/semantic_map/src/semantic_map_recompute.cpp @@ -333,6 +333,118 @@ void fixXml(std::string sweep_xml){ delete xmlWriter; } +std::string outtopic = "/some/topic"; +ros::Publisher out_pub; +void processMetaroom(std::string path){ + printf("processing: %s\n",path.c_str()); + + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + std::vector viewrgbs; + std::vector viewdepths; + std::vector viewtfs; + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); + for (auto objectFile : objectFiles){ + auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); + for (unsigned int i=0; iheight,cloud->width,CV_8UC3); + unsigned char * rgbdata = (unsigned char *)rgb.data; + + cv::Mat depth; + depth.create(cloud->height,cloud->width,CV_16UC1); + unsigned short * depthdata = (unsigned short *)depth.data; + + unsigned int nr_data = cloud->height * cloud->width; + for(unsigned int j = 0; j < nr_data; j++){ + PointType p = cloud->points[j]; + rgbdata[3*j+0] = p.b; + rgbdata[3*j+1] = p.g; + rgbdata[3*j+2] = p.r; + depthdata[j] = short(5000.0 * p.z); + } + + viewrgbs.push_back(rgb); + viewdepths.push_back(depth); + viewtfs.push_back(object.vAdditionalViewsTransforms[i]); + //viewcams.push_back(roomData.vIntermediateRoomCloudCamParams.front()); + + cv::namedWindow("rgbimage", cv::WINDOW_AUTOSIZE); + cv::imshow( "rgbimage", rgb); + cv::namedWindow("depthimage", cv::WINDOW_AUTOSIZE); + cv::imshow( "depthimage", depth); + cv::waitKey(30); + + } + } + + std::vector unrefined; + std::vector times; + for(unsigned int i = 0; i < viewrgbs.size(); i++){ + geometry_msgs::TransformStamped msg; + tf::transformStampedTFToMsg(viewtfs[i], msg); + long sec = msg.header.stamp.sec; + long nsec = msg.header.stamp.nsec; + double time = double(sec)+1e-9*double(nsec); + + Eigen::Quaterniond q; + q.w() = msg.transform.rotation.w; + q.x() = msg.transform.rotation.x; + q.y() = msg.transform.rotation.y; + q.z() = msg.transform.rotation.z; + + Eigen::Affine3d a (q); + a(0,3) = msg.transform.translation.x; + a(1,3) = msg.transform.translation.y; + a(2,3) = msg.transform.translation.z; + + + unrefined.push_back(a.matrix()); + times.push_back(time); + } + +/* + xmlWriter->writeStartElement("Transform"); + xmlWriter->writeStartElement("Translation"); + xmlWriter->writeStartElement("x"); + xmlWriter->writeCharacters(QString::number(msg.transform.translation.x)); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("y"); + xmlWriter->writeCharacters(QString::number(msg.transform.translation.y)); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("z"); + xmlWriter->writeCharacters(QString::number(msg.transform.translation.z)); + xmlWriter->writeEndElement(); + xmlWriter->writeEndElement(); // Translation + xmlWriter->writeStartElement("Rotation"); + xmlWriter->writeStartElement("w"); + xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w)); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("x"); + xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x)); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("y"); + xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y)); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("z"); + xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z)); +*/ + std_msgs::String msg; + msg.data = path; + out_pub.publish(msg); + ros::spinOnce(); +} + +void chatterCallback(const std_msgs::String::ConstPtr& msg){ + processMetaroom(msg->data); +} + void segmentWithAdditionalViewsXml(std::string sweep_xml,std::string prev){ printf("segmentWithAdditionalViewsXml\n"); // std::string prev = getPrevXML(sweep_xml); @@ -341,37 +453,66 @@ void segmentWithAdditionalViewsXml(std::string sweep_xml,std::string prev){ int main(int argc, char **argv) { - for(int ar = 1; ar < argc; ar++){ - std::string overall_folder = std::string(argv[ar]); - printf("overall folder: %s\n",overall_folder.c_str()); - - std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); - for (int i = 0; i < sweep_xmls.size(); i++){//auto sweep_xml : sweep_xmls) { - printf("current xml: %s\n",sweep_xmls[i].c_str()); - fixXml(sweep_xmls[i]); - if(i > 1){ - segmentWithAdditionalViewsXml(sweep_xmls[i-1],sweep_xmls[i]); - } - } - } - - printf("test\n"); - exit(0); - // Set up ROS. - ros::init(argc, argv, "Semantic_map_node"); - ros::NodeHandle n; - - ros::NodeHandle aRosNode("~"); - printf("test\n"); - //SemanticMapNode aSemanticMapNode(aRosNode); - SemanticMapNode aSemanticMapNode(n); - aSemanticMapNode.processRoomObservation(argv[1]); - exit(0); - printf("test\n"); - ros::Rate loop_rate(10); - while (ros::ok()) - { - ros::spinOnce(); - loop_rate.sleep(); - } + ros::init(argc, argv, "metaroom_additional_view_processing"); + ros::NodeHandle n; + + + int inputstate = 0; + for(int i = 1; i < argc;i++){ + printf("input: %s\n",argv[i]); + if(std::string(argv[i]).compare("-file") == 0){inputstate = 2;} + else if(std::string(argv[i]).compare("-intopic") == 0){inputstate = 0;} + else if(std::string(argv[i]).compare("-outtopic") == 0){inputstate = 1;} + else if(inputstate == 0){ + out_pub = n.advertise(outtopic, 1000); + ros::Subscriber sub = n.subscribe(std::string(argv[i]), 1000, chatterCallback); + ros::spin(); + }else if(inputstate == 1){ + outtopic = std::string(argv[i]); + }else{ + + out_pub = n.advertise(outtopic, 1000); + processMetaroom(std::string(argv[i])); + } + } + +// std::string listentopic = "/some/stupid/topic"; +// if(argc == 2){listentopic = std::string(argv[1]);} +// else{printf("robot listener: incorrect number of args. Should take a single arguement with a topicname!\n");} + + + +// for(int ar = 1; ar < argc; ar++){ +// std::string overall_folder = std::string(argv[ar]); +// printf("overall folder: %s\n",overall_folder.c_str()); + +// std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); +// for (int i = 0; i < sweep_xmls.size(); i++){//auto sweep_xml : sweep_xmls) { +// printf("current xml: %s\n",sweep_xmls[i].c_str()); +// fixXml(sweep_xmls[i]); +// if(i > 1){ +// segmentWithAdditionalViewsXml(sweep_xmls[i-1],sweep_xmls[i]); +// } +// } +// } + +// printf("test\n"); +// exit(0); +// // Set up ROS. +// ros::init(argc, argv, "Semantic_map_node"); +// ros::NodeHandle n; + +// ros::NodeHandle aRosNode("~"); +// printf("test\n"); +// //SemanticMapNode aSemanticMapNode(aRosNode); +// SemanticMapNode aSemanticMapNode(n); +// aSemanticMapNode.processRoomObservation(argv[1]); +// exit(0); +// printf("test\n"); +// ros::Rate loop_rate(10); +// while (ros::ok()) +// { +// ros::spinOnce(); +// loop_rate.sleep(); +// } } From fabb8a7bf7bb14deecfc76fe42e1213ed7481b13 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Fri, 19 Aug 2016 14:24:29 +0200 Subject: [PATCH 068/255] Managed to get incremental segmentation working in ROS --- .../launch/retrieval.launch | 2 + .../src/quasimodo_retrieval_node.cpp | 115 ++++++++++++++---- 2 files changed, 92 insertions(+), 25 deletions(-) diff --git a/quasimodo/quasimodo_retrieval/launch/retrieval.launch b/quasimodo/quasimodo_retrieval/launch/retrieval.launch index f00013ce..b9f3b4d9 100644 --- a/quasimodo/quasimodo_retrieval/launch/retrieval.launch +++ b/quasimodo/quasimodo_retrieval/launch/retrieval.launch @@ -1,10 +1,12 @@ + + diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index d1f2375d..e6038053 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -66,7 +66,8 @@ class retrieval_node { string retrieval_output; string retrieval_input; - VocabularyT vt; + //VocabularyT vt; + grouped_vocabulary_tree vt; dynamic_object_retrieval::vocabulary_summary summary; double iss_model_resolution; // 0.004 @@ -133,7 +134,7 @@ class retrieval_node { pubs[i] = n.advertise(string("/retrieval_cloud/") + to_string(i), 1, true); } sub = n.subscribe(retrieval_input, 10, &retrieval_node::run_retrieval, this); - service = n.advertiseService("/query_normal_cloud", &retrieval_node::service_callback, this); + //service = n.advertiseService("/query_normal_cloud", &retrieval_node::service_callback, this); /* vector folder_xmls = semantic_map_load_utilties::getSweepXmls(data_path.string(), true); for (const string& xml : folder_xmls) { @@ -405,6 +406,7 @@ class retrieval_node { std::cout << "Number of features: " << pfhrgb_cloud.size() << std::endl; } + /* bool service_callback(quasimodo_msgs::simple_query_cloud::Request& req, quasimodo_msgs::simple_query_cloud::Response& res) { @@ -526,6 +528,62 @@ class retrieval_node { cout << "Finished retrieval..." << endl; } + */ + + // to have this templated is totally unnecessary but whatever + void prune_results(std::pair::result_type> >, + std::vector::result_type> > >& results) + { + size_t counter = 0; + while (counter < std::min(number_query, int32_t(results.first.size()))) { + // assume that everything in mongodb is kept + if (!boost::filesystem::exists(results.first[counter].first)) { + auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair& v) { + return boost::filesystem::exists(v.first); + }); + if (iter == results.first.end()) { + break; + } + std::swap(results.first[counter], *iter); + } + else { + ++counter; + } + } + results.first.resize(counter); + } + + void prune_results(std::pair, grouped_vocabulary_tree::result_type> >, + std::vector, grouped_vocabulary_tree::result_type> > >& results) + { + size_t counter = 0; + while (counter < std::min(number_query, int32_t(results.first.size()))) { + // assume that everything in mongodb is kept + if (!boost::filesystem::exists(results.first[counter].first[0])) { + auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair, grouped_vocabulary_tree::result_type>& v) { + return boost::filesystem::exists(v.first[0]); + }); + if (iter == results.first.end()) { + break; + } + std::swap(results.first[counter], *iter); + } + else { + ++counter; + } + } + results.first.resize(counter); + } + + boost::filesystem::path base_path(const boost::filesystem::path& path) + { + return path; + } + + boost::filesystem::path base_path(const vector& path) + { + return path[0]; + } void run_retrieval(const quasimodo_msgs::retrieval_query::ConstPtr& query_msg) //const string& sweep_xml) { @@ -582,27 +640,19 @@ class retrieval_node { vector sweep_paths; //auto results = dynamic_object_retrieval::query_reweight_vocabulary(vt, refined_query, query_image, query_depth, // K, number_query, vocabulary_path, summary, false); - auto results = dynamic_object_retrieval::query_reweight_vocabulary((vocabulary_tree&)vt, features, 200, vocabulary_path, summary, true, kind); + /* + * This should be fine for grouped queries, what I was using in the paper + * The exists next will have to be changed somehow + * Then load_retrieved_clouds is exactly the same that I used in the paper + * + * + */ + auto results = dynamic_object_retrieval::query_reweight_vocabulary((VocabularyT&)vt, features, 200, vocabulary_path, summary, true, kind); // This is just to make sure that we have valid results even when some meta rooms have been deleted - size_t counter = 0; - while (counter < std::min(number_query, int32_t(results.first.size()))) { - // assume that everything in mongodb is kept - if (!boost::filesystem::exists(results.first[counter].first)) { - auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair::result_type>& v) { - return boost::filesystem::exists(v.first); - }); - if (iter == results.first.end()) { - break; - } - std::swap(results.first[counter], *iter); - } - else { - ++counter; - } - } - results.first.resize(counter); + prune_results(results); + // this is OK, both return the clouds and paths tie(retrieved_clouds, sweep_paths) = benchmark_retrieval::load_retrieved_clouds(results.first); cout << "Sweep paths:" << endl; @@ -614,7 +664,7 @@ class retrieval_node { // FIXME: This is a hack that requires that at least one is not from MongoDB, needs fixing! tf::StampedTransform room_transform; for (int i = 0; i < retrieved_clouds.size(); ++i) { - string name = results.first[i].first.stem().string(); + string name = base_path(results.first[i].first).stem().string(); cout << name << endl; if (name != "segment") { auto data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); @@ -632,7 +682,7 @@ class retrieval_node { vector scores; vector indices; for (auto s : results.first) { - boost::filesystem::path segment_path = s.first; + boost::filesystem::path segment_path = base_path(s.first); string name = segment_path.stem().string(); cout << name << endl; if (name == "segment") { // for the mongodb results @@ -655,7 +705,7 @@ class retrieval_node { vector inds; vector > sweep_transforms; if (indices[i] == -1) { // special case for mongodb result - tie(masks[i], images[i], depths[i], inds) = generate_images_for_mongodb_object(retrieved_clouds[i], K, boost::filesystem::path(results.first[i].first), sweep_transforms); + tie(masks[i], images[i], depths[i], inds) = generate_images_for_mongodb_object(retrieved_clouds[i], K, base_path(results.first[i].first), sweep_transforms); } else { auto sweep_data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); @@ -802,9 +852,24 @@ int main(int argc, char** argv) { ros::init(argc, argv, "quasimodo_retrieval_node"); - retrieval_node > rs(ros::this_node::getName()); + ros::NodeHandle pn("~"); + bool enable_incremental; + pn.param("enable_incremental", enable_incremental, false); - ros::spin(); + if (enable_incremental) { + + retrieval_node > rs(ros::this_node::getName()); + + ros::spin(); + + } + else { + + retrieval_node > rs(ros::this_node::getName()); + + ros::spin(); + + } return 0; } From 55b1331212d649e8ca505cc3bfe03e3b29f04296 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Fri, 19 Aug 2016 15:21:16 +0200 Subject: [PATCH 069/255] Changed defualt to wo incremental! --- quasimodo/quasimodo_retrieval/launch/retrieval.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_retrieval/launch/retrieval.launch b/quasimodo/quasimodo_retrieval/launch/retrieval.launch index b9f3b4d9..086ba2e2 100644 --- a/quasimodo/quasimodo_retrieval/launch/retrieval.launch +++ b/quasimodo/quasimodo_retrieval/launch/retrieval.launch @@ -1,7 +1,7 @@ - + From 0fdefbe41e5b9b41af83a578b2a93ab4fc7f89f5 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 19 Aug 2016 17:10:52 +0200 Subject: [PATCH 070/255] working in integrating segmentation of additional views --- .../metaroom_xml_parser/simple_xml_parser.h | 2 +- quasimodo/quasimodo_brain/CMakeLists.txt | 6 + quasimodo/quasimodo_brain/src/Util/Util.cpp | 158 ++++-- quasimodo/quasimodo_brain/src/Util/Util.h | 1 + .../metaroom_additional_view_processing.cpp | 487 ++++++++++++++++++ .../quasimodo_models/include/core/Camera.h | 1 + .../quasimodo_models/include/model/Model.h | 4 + .../quasimodo_models/src/core/Camera.cpp | 10 + .../quasimodo_models/src/model/Model.cpp | 12 + .../src/registration/MassRegistrationPPR2.cpp | 24 +- semantic_map/CMakeLists.txt | 20 +- semantic_map/src/semantic_map_recompute.cpp | 70 ++- 12 files changed, 703 insertions(+), 92 deletions(-) create mode 100644 quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp diff --git a/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h b/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h index 7ff86883..91564460 100644 --- a/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h +++ b/metaroom_xml_parser/include/metaroom_xml_parser/simple_xml_parser.h @@ -253,7 +253,7 @@ friend class SimpleDynamicObjectParser; } if (xmlReader->name() == "RoomRunNumber") { - QString tmp = xmlReader->readElementText(); + QString tmp = xmlReader->readElementText(); aRoom.roomRunNumber = atoi(tmp.toStdString().c_str());//xmlReader->readElementText().toInt(); } diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 9a5f2c38..485ef362 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -156,6 +156,12 @@ add_executable( segmentationserver src/segmentationserver.cpp) add_dependencies( segmentationserver roscpp quasimodo_msgs_generate_messages_cpp) target_link_libraries( segmentationserver quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + +add_executable( metaroom_additional_view_processing src/metaroom_additional_view_processing.cpp) +add_dependencies( metaroom_additional_view_processing roscpp quasimodo_msgs_generate_messages_cpp) +target_link_libraries( metaroom_additional_view_processing quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + + ############# ## Install ## ############# diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 3a68fdb4..ac928d60 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -126,6 +126,21 @@ std::vector getRegisteredViewPoses(const std::string& poses_fil return toRet; } +Eigen::Matrix4d getMat(tf::StampedTransform tf){ + //Transform + geometry_msgs::TransformStamped tfstmsg; + tf::transformStampedTFToMsg (tf, tfstmsg); + geometry_msgs::Transform tfmsg = tfstmsg.transform; + geometry_msgs::Pose pose; + pose.orientation = tfmsg.rotation; + pose.position.x = tfmsg.translation.x; + pose.position.y = tfmsg.translation.y; + pose.position.z = tfmsg.translation.z; + Eigen::Affine3d epose; + tf::poseMsgToEigen(pose, epose); + return epose.matrix(); +} + reglib::Model * load_metaroom_model(std::string sweep_xml){ int slash_pos = sweep_xml.find_last_of("/"); std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; @@ -136,6 +151,9 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ reglib::Model * sweepmodel = 0; + Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); + cout << m2 << endl << endl; + std::vector current_room_frames; for (size_t i=0; icy = 236.842039; - cout<<"Intermediate cloud size "<points.size()<points.size()< models, std::vec mu->massreg_timeout = 60*4; mu->viewer = viewer; + std::vector bg_po; + std::vector bg_fr; + std::vector bg_mm; + bg->getData(bg_po, bg_fr, bg_mm); + bg->points = mu->getSuperPoints(bg_po,bg_fr,bg_mm,1,false); + + std::vector< std::vector > mod_po_vec; + std::vector< std::vector > mod_fr_vec; + std::vector< std::vector > mod_mm_vec; + + for(int j = 0; j < models.size(); j++){ + reglib::Model * mod = models[j]; + std::vector mod_po; + std::vector mod_fr; + std::vector mod_mm; + mod->getData(mod_po, mod_fr, mod_mm); + mod->points = mu->getSuperPoints(mod_po,mod_fr,mod_mm,1,false); + + mod_po_vec.push_back(mod_po); + mod_fr_vec.push_back(mod_fr); + mod_mm_vec.push_back(mod_mm); + } if(models.size() > 0 && bg->frames.size() > 0){ std::vector cpmod; - bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + cpmod.push_back(Eigen::Matrix4d::Identity());//,models.front()->relativeposes.front().inverse() * bg->relativeposes.front()); massregmod->addModel(bg); for(int j = 0; j < models.size(); j++){ - models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - cpmod.push_back(bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); - massregmod->addModel(models[j]); + //models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + //cpmod.push_back(bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); + //massregmod->addModel(models[j]); + + reglib::Model * mod = models[j]; +// std::vector mod_po; +// std::vector mod_fr; +// std::vector mod_mm; +// mod->getData(mod_po, mod_fr, mod_mm); + cpmod.push_back(bg_po.front().inverse() * mod_po_vec[j].front()); + massregmod->addModel(mod); } reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); for(int j = 0; j < models.size(); j++){ Eigen::Matrix4d change = mfrmod.poses[j+1] * cpmod[j+1].inverse(); - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + + for(unsigned int k = 0; k < mod_po_vec[j].size(); k++){ + mod_po_vec[j][k] = change*mod_po_vec[j][k]; } + +// for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ +// models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; +// } + +// for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ +// models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; +// } } }else if(models.size() > 1){ std::vector cpmod; + //Eigen::Matrix4d first = Eigen::Matrix4d::Identity(); + Eigen::Matrix4d first = mod_po_vec.front().front(); for(int j = 0; j < models.size(); j++){ - models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); - massregmod->addModel(models[j]); +// models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); +// cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); +// massregmod->addModel(models[j]); + + reglib::Model * mod = models[j]; +// std::vector mod_po; +// std::vector mod_fr; +// std::vector mod_mm; +// mod->getData(mod_po, mod_fr, mod_mm); +// mod->points = mu->getSuperPoints(mod_po,mod_fr,mod_mm,1,false); +// if(j == 0){first = mod_po.front();} + cpmod.push_back(first.inverse() * mod_po_vec[j].front()); + massregmod->addModel(mod); } reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); for(int j = 0; j < models.size(); j++){ Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + + for(unsigned int k = 0; k < mod_po_vec[j].size(); k++){ + mod_po_vec[j][k] = change*mod_po_vec[j][k]; } +// for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ +// models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; +// } + +// for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ +// models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; +// } } } delete massregmod; - std::vector bgcp; - std::vector bgcf; + std::vector bgcp = bg_po; + std::vector bgcf = bg_fr; std::vector bgmask; - for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ - bgcp.push_back(bg->relativeposes[k]); - bgcf.push_back(bg->frames[k]); - bgmask.push_back(bg->modelmasks[k]->getMask()); + for(unsigned int k = 0; k < bg_mm.size(); k++){ + bgmask.push_back(bg_mm[k]->getMask()); } +// for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ +// bgcp.push_back(bg->relativeposes[k]); +// bgcf.push_back(bg->frames[k]); +// bgmask.push_back(bg->modelmasks[k]->getMask()); +// } + for(int j = 0; j < models.size(); j++){ reglib::Model * model = models[j]; @@ -279,7 +370,8 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec } //mu->computeMovingDynamicStatic(bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions - + print("TODO: %s :: %i \n",__FILE__,__LINE__); +exit(0); std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,false);//Determine external occlusions diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index a502fc70..b7386fd9 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -42,6 +42,7 @@ quasimodo_msgs::model getModelMSG(reglib::Model * model); std::vector getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms); +Eigen::Matrix4d getMat(tf::StampedTransform tf); reglib::Model * load_metaroom_model(std::string sweep_xml); void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg = false); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp new file mode 100644 index 00000000..293ed48a --- /dev/null +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -0,0 +1,487 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include +#include + +#include +#include + +#include "eigen_conversions/eigen_msg.h" +#include "tf_conversions/tf_eigen.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" +#include "metaroom_xml_parser/simple_summary_parser.h" + +#include + +#include "ros/ros.h" +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" +#include "quasimodo_msgs/segment_model.h" +#include "quasimodo_msgs/metaroom_pair.h" + +#include "ros/ros.h" +#include +#include +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" +#include "metaroom_xml_parser/load_utilities.h" + +#include +#include + +#include "modelupdater/ModelUpdater.h" +#include "core/RGBDFrame.h" +#include "Util/Util.h" + +ros::ServiceClient segmentation_client; +using namespace std; +using namespace semantic_map_load_utilties; + +typedef pcl::PointXYZRGB PointType; +typedef pcl::PointCloud Cloud; +typedef typename Cloud::Ptr CloudPtr; +typedef pcl::search::KdTree Tree; +typedef semantic_map_load_utilties::DynamicObjectData ObjectData; + +using namespace std; +using namespace semantic_map_load_utilties; + +/* +bool segment_metaroom(quasimodo_msgs::metaroom_pair::Request & req, quasimodo_msgs::metaroom_pair::Response & res){ + printf("segment_metaroom\n"); + + printf("background: %s\n",req.background.c_str()); + printf("foreground: %s\n",req.foreground.c_str()); + + reglib::Model * bg_model = quasimodo_brain::load_metaroom_model(req.background); + reglib::Model * fg_model = quasimodo_brain::load_metaroom_model(req.foreground); + + quasimodo_msgs::segment_model sm; + sm.request.backgroundmodel = quasimodo_brain::getModelMSG(bg_model); + sm.request.models.push_back(quasimodo_brain::getModelMSG(fg_model)); + + bool status; + + if (segmentation_client.call(sm)){ + if(sm.response.dynamicmasks.size() > 0){ + res.dynamicmasks = sm.response.dynamicmasks.front().images; + res.movingmasks = sm.response.movingmasks.front().images; + } + status = true; + }else{ + ROS_ERROR("Failed to call service segment_model"); + status = false; + } + + bg_model->fullDelete(); + delete bg_model; + fg_model->fullDelete(); + delete fg_model; + return status; +} +*/ + +std::string overall_folder = "~/.semanticMap/"; +boost::shared_ptr viewer; +int visualization_lvl = 0; +std::string outtopic = "/some/topic"; +ros::Publisher out_pub; + +void processMetaroom(std::string path){ + printf("processing: %s\n",path.c_str()); + + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + std::vector viewrgbs; + std::vector viewdepths; + std::vector viewtfs; + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); + for (auto objectFile : objectFiles){ + auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); + for (unsigned int i=0; iheight,cloud->width,CV_8UC3); + unsigned char * rgbdata = (unsigned char *)rgb.data; + + cv::Mat depth; + depth.create(cloud->height,cloud->width,CV_16UC1); + unsigned short * depthdata = (unsigned short *)depth.data; + + unsigned int nr_data = cloud->height * cloud->width; + for(unsigned int j = 0; j < nr_data; j++){ + PointType p = cloud->points[j]; + rgbdata[3*j+0] = p.b; + rgbdata[3*j+1] = p.g; + rgbdata[3*j+2] = p.r; + depthdata[j] = short(5000.0 * p.z); + } + + viewrgbs.push_back(rgb); + viewdepths.push_back(depth); + viewtfs.push_back(object.vAdditionalViewsTransforms[i]); + //viewcams.push_back(roomData.vIntermediateRoomCloudCamParams.front()); + + cv::namedWindow("rgbimage", cv::WINDOW_AUTOSIZE); + cv::imshow( "rgbimage", rgb); + cv::namedWindow("depthimage", cv::WINDOW_AUTOSIZE); + cv::imshow( "depthimage", depth); + cv::waitKey(30); + + } + } + + reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path); + + + std::vector frames; + std::vector masks; + std::vector unrefined; + + std::vector both_unrefined; + both_unrefined.push_back(Eigen::Matrix4d::Identity()); + std::vector times; + for(unsigned int i = 0; i < viewrgbs.size(); i++){ + printf("additional view: %i\n",i); + geometry_msgs::TransformStamped msg; + tf::transformStampedTFToMsg(viewtfs[i], msg); + long sec = msg.header.stamp.sec; + long nsec = msg.header.stamp.nsec; + double time = double(sec)+1e-9*double(nsec); + + Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]); + + cout << m << endl << endl; + + unrefined.push_back(m); + times.push_back(time); + + cv::Mat fullmask; + fullmask.create(480,640,CV_8UC1); + unsigned char * maskdata = (unsigned char *)fullmask.data; + for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} + masks.push_back(new reglib::ModelMask(fullmask)); + + reglib::Camera * cam = sweep->frames.front()->camera->clone(); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m);//a.matrix()); + frames.push_back(frame); + + //both_unrefined.push_back(sweep->frames.front()->pose.inverse()*a.matrix()); + both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); + } + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( sweep, reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + + sweep->points = mu->getSuperPoints(sweep->relativeposes,sweep->frames,sweep->modelmasks,1,false); + + + + //Not needed if metaroom well calibrated + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); + bgmassreg->timeout = 20; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = false; + bgmassreg->visualizationLvl = visualization_lvl; + bgmassreg->maskstep = 15; + bgmassreg->nomaskstep = 15; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->addModel(sweep); + bgmassreg->setData(frames,masks); + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); + + + SimpleXMLParser parser; + SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); + std::string current_waypointid = current_roomData.roomWaypointId; + + if(overall_folder.back() == '/'){overall_folder.pop_back();} + + + int prevind = -1; + std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + for (int i = 0; i < sweep_xmls.size(); i++){ + + SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); + std::string other_waypointid = other_roomData.roomWaypointId; + + + if(sweep_xmls[i].compare(path) == 0){ + break; + } + if(other_waypointid.compare(current_waypointid) == 0){ + prevind = i; + } + } + + if(prevind != -1){ + std::string prev = sweep_xmls[prevind]; + printf("prev: %s\n",prev.c_str()); + + reglib::Model * bg = quasimodo_brain::load_metaroom_model(prev); + bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + + std::vector< reglib::Model * > models; + models.push_back(sweep); + + + std::vector< std::vector< cv::Mat > > internal; + std::vector< std::vector< cv::Mat > > external; + std::vector< std::vector< cv::Mat > > dynamic; + + quasimodo_brain::segment(bg,models,internal,external,dynamic); + + + + for(unsigned int i = 0; visualization_lvl > 0 && i < models.size(); i++){ + std::vector internal_masks = internal[i]; + std::vector external_masks = external[i]; + std::vector dynamic_masks = dynamic[i]; + reglib::Model * model = models[i]; + + + std::vector mod_po; + std::vector mod_fr; + std::vector mod_mm; + model->getData(mod_po, mod_fr, mod_mm); + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + +// for(unsigned int j = 0; j < model->frames.size(); j++){ +// reglib::RGBDFrame * frame = model->frames[j]; +// Eigen::Matrix4d p = model->relativeposes[j]; + for(unsigned int j = 0; j < mod_fr.size(); j++){ + reglib::RGBDFrame * frame = mod_fr[j]; + Eigen::Matrix4d p = mod_po[j]; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + reglib::Camera * camera = frame->camera; + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); + unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); + + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + pcl::PointXYZRGBNormal point; + point.x = m00*x + m01*y + m02*z + m03; + point.y = m10*x + m11*y + m12*z + m13; + point.z = m20*x + m21*y + m22*z + m23; + + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; + + if(dynamicmaskdata[ind] != 0){ + point.b = 0; + point.g = 255; + point.r = 0; + }else if(internalmaskdata[ind] == 0){ + point.b = 0; + point.g = 0; + point.r = 255; + } + + cloud->points.push_back(point); + } + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + viewer->spin(); + //while(cv::waitKey(50)!='q'){viewer->spinOnce();} + } + + } + + + + delete bgmassreg; + delete reg; + delete mu; + +// xmlWriter->writeStartElement("Transform"); +// xmlWriter->writeStartElement("Translation"); +// xmlWriter->writeStartElement("x"); +// xmlWriter->writeCharacters(QString::number(msg.transform.translation.x)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("y"); +// xmlWriter->writeCharacters(QString::number(msg.transform.translation.y)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("z"); +// xmlWriter->writeCharacters(QString::number(msg.transform.translation.z)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeEndElement(); // Translation +// xmlWriter->writeStartElement("Rotation"); +// xmlWriter->writeStartElement("w"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("x"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("y"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("z"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z)); + + std_msgs::String msg; + msg.data = path; + out_pub.publish(msg); + ros::spinOnce(); +} + +void chatterCallback(const std_msgs::String::ConstPtr& msg){ + processMetaroom(msg->data); +} + +void segmentWithAdditionalViewsXml(std::string sweep_xml,std::string prev){ + printf("segmentWithAdditionalViewsXml\n"); +// std::string prev = getPrevXML(sweep_xml); +} + +int main(int argc, char** argv){ + ros::init(argc, argv, "metaroom_additional_view_processing"); + ros::NodeHandle n; + + + + int inputstate = 0; + for(int i = 1; i < argc;i++){ + printf("input: %s\n",argv[i]); + if(std::string(argv[i]).compare("-file") == 0){inputstate = 2;} + else if(std::string(argv[i]).compare("-intopic") == 0){inputstate = 0;} + else if(std::string(argv[i]).compare("-outtopic") == 0){inputstate = 1;} + else if(std::string(argv[i]).compare("-folder") == 0){inputstate = 4;} + else if(std::string(argv[i]).compare("-v") == 0){ + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + visualization_lvl = 1; + inputstate = 3; + }else if(inputstate == 0){ + out_pub = n.advertise(outtopic, 1000); + ros::Subscriber sub = n.subscribe(std::string(argv[i]), 1000, chatterCallback); + ros::spin(); + }else if(inputstate == 1){ + outtopic = std::string(argv[i]); + }else if(inputstate == 3){ + visualization_lvl = atoi(argv[i]); + }else if(inputstate == 4){ + overall_folder = std::string(argv[i]); + }else{ + out_pub = n.advertise(outtopic, 1000); + processMetaroom(std::string(argv[i])); + } + } + /* + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + + + + for(int ar = 1; ar < argc; ar++){ + string overall_folder = std::string(argv[ar]); + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + printf("sweep_xmls\n"); + for (auto sweep_xml : sweep_xmls) { + printf("sweep_xml: %s\n",sweep_xml.c_str()); + load2(sweep_xml); + } + } + + + + + ros::init(argc, argv, "test_segment"); + ros::NodeHandle n; + ros::ServiceClient segmentation_client = n.serviceClient("segment_model"); + + for(unsigned int i = 1; i < models.size(); i++){ + quasimodo_msgs::segment_model sm; + sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); + + if(i > 0){ + sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]); + } + + if (segmentation_client.call(sm)){//Build model from frame + //int model_id = mff.response.model_id; + printf("segmented: %i\n",i); + }else{ROS_ERROR("Failed to call service segment_model");} + } + //ros::spin(); + + delete reg; + for(size_t j = 0; j < models.size(); j++){ + models[j]->fullDelete(); + delete models[j]; + } + printf("done\n"); + */ + return 0; +} diff --git a/quasimodo/quasimodo_models/include/core/Camera.h b/quasimodo/quasimodo_models/include/core/Camera.h index a35ea6dd..011e78d0 100644 --- a/quasimodo/quasimodo_models/include/core/Camera.h +++ b/quasimodo/quasimodo_models/include/core/Camera.h @@ -37,6 +37,7 @@ namespace reglib void save(std::string path = ""); void print(); static Camera * load(std::string path); + Camera * clone(); }; } diff --git a/quasimodo/quasimodo_models/include/model/Model.h b/quasimodo/quasimodo_models/include/model/Model.h index 3c229360..d6aa5570 100644 --- a/quasimodo/quasimodo_models/include/model/Model.h +++ b/quasimodo/quasimodo_models/include/model/Model.h @@ -118,6 +118,10 @@ class superpoint{ void save(std::string path = ""); static Model * load(Camera * cam, std::string path); bool testFrame(int ind = 0); + + void getData(std::vector & po, std::vector & fr, std::vector & mm, Eigen::Matrix4d p = Eigen::Matrix4d::Identity()); + + //void getData(std::vector & po, std::vector & fr, std::vector & mm, Eigen::Matrix4d p = Eigen::Matrix4d::Identity()); }; } diff --git a/quasimodo/quasimodo_models/src/core/Camera.cpp b/quasimodo/quasimodo_models/src/core/Camera.cpp index 42ccac0e..31aad614 100644 --- a/quasimodo/quasimodo_models/src/core/Camera.cpp +++ b/quasimodo/quasimodo_models/src/core/Camera.cpp @@ -66,6 +66,16 @@ void Camera::print(){ printf("Camera fx: %5.5f fy: %5.5f cx: %5.5f cy: %5.5f\n",fx,fy,cx,cy); } + +Camera * Camera::clone(){ + Camera * cam = new Camera(); + cam->fx = fx; + cam->fy = fy; + cam->cx = cx; + cam->cy = cy; + return cam; +} + Camera * Camera::load(std::string path){ Camera * cam = new Camera(); diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index a633ba1e..1493474c 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -32,6 +32,18 @@ Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){ //recomputeModelPoints(); } +void Model::getData(std::vector & po, std::vector & fr, std::vector & mm, Eigen::Matrix4d p){ + for(unsigned int i = 0; i < frames.size(); i++){ + fr.push_back(frames[i]); + mm.push_back(modelmasks[i]); + po.push_back(p*relativeposes[i]); + } + + for(unsigned int i = 0; i < submodels.size(); i++){ + submodels[i]->getData(po,fr,mm, p*submodels_relativeposes[i]); + } +} + void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask){ bool * maskvec = modelmask->maskvec; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 75894c64..20375157 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -507,20 +507,20 @@ void MassRegistrationPPR2::setData(std::vector frames_,std::vector 0){ - for(unsigned int i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} - arraypoints.resize(0); - } +// if(arraypoints.size() > 0){ +// for(unsigned int i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} +// arraypoints.resize(0); +// } - if(a3dv.size() > 0){ - for(unsigned int i = 0; i < a3dv.size(); i++){delete a3dv[i];} - a3dv.resize(0); - } +// if(a3dv.size() > 0){ +// for(unsigned int i = 0; i < a3dv.size(); i++){delete a3dv[i];} +// a3dv.resize(0); +// } - if(trees3d.size() > 0){ - for(unsigned int i = 0; i < trees3d.size(); i++){delete trees3d[i];} - trees3d.resize(0); - } +// if(trees3d.size() > 0){ +// for(unsigned int i = 0; i < trees3d.size(); i++){delete trees3d[i];} +// trees3d.resize(0); +// } for(unsigned int i = 0; i < nr_frames; i++){addData(frames_[i], mmasks_[i]);} //printf("total load time: %5.5f\n",getTime()-total_load_time_start); diff --git a/semantic_map/CMakeLists.txt b/semantic_map/CMakeLists.txt index 01f85ccb..3f489311 100644 --- a/semantic_map/CMakeLists.txt +++ b/semantic_map/CMakeLists.txt @@ -94,13 +94,13 @@ set(SRCS add_library(semantic_map ${HDRS} ${HDRS_IMPL} ${SRCS}) add_executable(semantic_map_node include/semantic_map/semantic_map_node.h src/semantic_map_node.cpp src/semantic_map_main.cpp) -add_executable(semantic_map_recompute include/semantic_map/semantic_map_node.h src/semantic_map_node.cpp src/semantic_map_recompute.cpp) +#add_executable(semantic_map_recompute include/semantic_map/semantic_map_node.h src/semantic_map_node.cpp src/semantic_map_recompute.cpp) add_executable(load_from_mongo src/load_from_mongo.cpp) add_executable(add_to_mongo src/add_to_mongo.cpp) add_dependencies(semantic_map quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) add_dependencies(semantic_map_node quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) -add_dependencies(semantic_map_recompute quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) +#add_dependencies(semantic_map_recompute quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) target_link_libraries(semantic_map ${catkin_LIBRARIES} @@ -117,13 +117,13 @@ add_dependencies(semantic_map_recompute quasimodo_msgs_generate_messages_cpp sem semantic_map ) - target_link_libraries(semantic_map_recompute - ${catkin_LIBRARIES} - ${PCL_LIBRARIES} - ${QT_LIBRARIES} - ${Boost_LIBRARIES} - semantic_map - ) + #target_link_libraries(semantic_map_recompute + # ${catkin_LIBRARIES} + # ${PCL_LIBRARIES} + # ${QT_LIBRARIES} + # ${Boost_LIBRARIES} + # semantic_map + # ) target_link_libraries(load_from_mongo ${catkin_LIBRARIES} @@ -143,7 +143,7 @@ add_dependencies(semantic_map_recompute quasimodo_msgs_generate_messages_cpp sem ############################# INSTALL TARGETS -install(TARGETS semantic_map semantic_map_node semantic_map_recompute load_from_mongo +install(TARGETS semantic_map semantic_map_node load_from_mongo #semantic_map_recompute ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/semantic_map/src/semantic_map_recompute.cpp b/semantic_map/src/semantic_map_recompute.cpp index 25e934cc..24817bf2 100644 --- a/semantic_map/src/semantic_map_recompute.cpp +++ b/semantic_map/src/semantic_map_recompute.cpp @@ -3,6 +3,9 @@ #include "eigen_conversions/eigen_msg.h" #include "tf_conversions/tf_eigen.h" + +#include "Util/Util.h" + typedef pcl::PointXYZRGB PointType; typedef pcl::PointCloud Cloud; typedef typename Cloud::Ptr CloudPtr; @@ -12,6 +15,8 @@ typedef semantic_map_load_utilties::DynamicObjectData ObjectData; using namespace std; using namespace semantic_map_load_utilties; +boost::shared_ptr viewer; + std::string getPrevXML(std::string xml_file_name){ std::cout<<"File name "<height,cloud->width,CV_8UC3); unsigned char * rgbdata = (unsigned char *)rgb.data; @@ -384,6 +388,8 @@ void processMetaroom(std::string path){ } } + reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path); + std::vector unrefined; std::vector times; for(unsigned int i = 0; i < viewrgbs.size(); i++){ @@ -407,49 +413,41 @@ void processMetaroom(std::string path){ unrefined.push_back(a.matrix()); times.push_back(time); + } -/* - xmlWriter->writeStartElement("Transform"); - xmlWriter->writeStartElement("Translation"); - xmlWriter->writeStartElement("x"); - xmlWriter->writeCharacters(QString::number(msg.transform.translation.x)); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("y"); - xmlWriter->writeCharacters(QString::number(msg.transform.translation.y)); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("z"); - xmlWriter->writeCharacters(QString::number(msg.transform.translation.z)); - xmlWriter->writeEndElement(); - xmlWriter->writeEndElement(); // Translation - xmlWriter->writeStartElement("Rotation"); - xmlWriter->writeStartElement("w"); - xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w)); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("x"); - xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x)); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("y"); - xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y)); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("z"); - xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z)); -*/ +// xmlWriter->writeStartElement("Transform"); +// xmlWriter->writeStartElement("Translation"); +// xmlWriter->writeStartElement("x"); +// xmlWriter->writeCharacters(QString::number(msg.transform.translation.x)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("y"); +// xmlWriter->writeCharacters(QString::number(msg.transform.translation.y)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("z"); +// xmlWriter->writeCharacters(QString::number(msg.transform.translation.z)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeEndElement(); // Translation +// xmlWriter->writeStartElement("Rotation"); +// xmlWriter->writeStartElement("w"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("x"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("y"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("z"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z)); + std_msgs::String msg; msg.data = path; out_pub.publish(msg); ros::spinOnce(); } -void chatterCallback(const std_msgs::String::ConstPtr& msg){ - processMetaroom(msg->data); -} - -void segmentWithAdditionalViewsXml(std::string sweep_xml,std::string prev){ - printf("segmentWithAdditionalViewsXml\n"); -// std::string prev = getPrevXML(sweep_xml); -} - +void chatterCallback(const std_msgs::String::ConstPtr& msg){processMetaroom(msg->data);} int main(int argc, char **argv) { From 216b6ad92f5c07af715e05a499af5824cab48836 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Sun, 21 Aug 2016 23:33:08 +0200 Subject: [PATCH 071/255] working on edges for image --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 933 +-- .../metaroom_additional_view_processing.cpp | 50 +- .../src/modelupdater/ModelUpdater.cpp | 5485 +++++++++-------- .../src/registration/MassRegistration.cpp | 6 + .../src/registration/MassRegistrationPPR2.cpp | 9 +- 5 files changed, 3318 insertions(+), 3165 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index ac928d60..cd507903 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -3,509 +3,532 @@ namespace quasimodo_brain { double getTime(){ - struct timeval start1; - gettimeofday(&start1, NULL); - return double(start1.tv_sec+(start1.tv_usec/1000000.0)); + struct timeval start1; + gettimeofday(&start1, NULL); + return double(start1.tv_sec+(start1.tv_usec/1000000.0)); } reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg){ - reglib::Model * model = new reglib::Model(); - - for(unsigned int i = 0; i < msg.local_poses.size(); i++){ - sensor_msgs::CameraInfo camera = msg.frames[i].camera; - ros::Time capture_time = msg.frames[i].capture_time; - geometry_msgs::Pose pose = msg.frames[i].pose; - - cv_bridge::CvImagePtr rgb_ptr; - try{ rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat rgb = rgb_ptr->image; - - cv_bridge::CvImagePtr depth_ptr; - try{ depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat depth = depth_ptr->image; - - Eigen::Affine3d epose; - tf::poseMsgToEigen(pose, epose); - - reglib::Camera * cam = new reglib::Camera(); - if(camera.K[0] > 0){ - cam->fx = camera.K[0]; - cam->fy = camera.K[4]; - cam->cx = camera.K[2]; - cam->cy = camera.K[5]; - } - - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); - model->frames.push_back(frame); - - geometry_msgs::Pose pose1 = msg.local_poses[i]; - Eigen::Affine3d epose1; - tf::poseMsgToEigen(pose1, epose1); - model->relativeposes.push_back(epose1.matrix()); - - cv_bridge::CvImagePtr mask_ptr; - try{ mask_ptr = cv_bridge::toCvCopy(msg.masks[i], sensor_msgs::image_encodings::MONO8);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat mask = mask_ptr->image; + reglib::Model * model = new reglib::Model(); + + for(unsigned int i = 0; i < msg.local_poses.size(); i++){ + sensor_msgs::CameraInfo camera = msg.frames[i].camera; + ros::Time capture_time = msg.frames[i].capture_time; + geometry_msgs::Pose pose = msg.frames[i].pose; + + cv_bridge::CvImagePtr rgb_ptr; + try{ rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat rgb = rgb_ptr->image; + + cv_bridge::CvImagePtr depth_ptr; + try{ depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat depth = depth_ptr->image; + + Eigen::Affine3d epose; + tf::poseMsgToEigen(pose, epose); + + reglib::Camera * cam = new reglib::Camera(); + if(camera.K[0] > 0){ + cam->fx = camera.K[0]; + cam->fy = camera.K[4]; + cam->cx = camera.K[2]; + cam->cy = camera.K[5]; + } + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); + model->frames.push_back(frame); + + geometry_msgs::Pose pose1 = msg.local_poses[i]; + Eigen::Affine3d epose1; + tf::poseMsgToEigen(pose1, epose1); + model->relativeposes.push_back(epose1.matrix()); + + cv_bridge::CvImagePtr mask_ptr; + try{ mask_ptr = cv_bridge::toCvCopy(msg.masks[i], sensor_msgs::image_encodings::MONO8);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat mask = mask_ptr->image; model->modelmasks.push_back(new reglib::ModelMask(mask)); - } - model->recomputeModelPoints(); - return model; + } + model->recomputeModelPoints(); + return model; } void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp){ - int startsize = msg.local_poses.size(); - msg.local_poses.resize(startsize+model->relativeposes.size()); - msg.frames.resize(startsize+model->frames.size()); - msg.masks.resize(startsize+model->modelmasks.size()); - for(unsigned int i = 0; i < model->relativeposes.size(); i++){ - geometry_msgs::Pose pose1; - tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i])*rp, pose1); - geometry_msgs::Pose pose2; - tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose)*rp, pose2); - cv_bridge::CvImage rgbBridgeImage; - rgbBridgeImage.image = model->frames[i]->rgb; - rgbBridgeImage.encoding = "bgr8"; - cv_bridge::CvImage depthBridgeImage; - depthBridgeImage.image = model->frames[i]->depth; - depthBridgeImage.encoding = "mono16"; - cv_bridge::CvImage maskBridgeImage; - maskBridgeImage.image = model->modelmasks[i]->getMask(); - maskBridgeImage.encoding = "mono8"; - msg.local_poses[startsize+i] = pose1; - msg.frames[startsize+i].capture_time = ros::Time(); - msg.frames[startsize+i].pose = pose2; - msg.frames[startsize+i].frame_id = model->frames[i]->id; - msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); - msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); - msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg());//getMask() - - msg.frames[startsize+i].camera.K[0] = model->frames[i]->camera->fx; - msg.frames[startsize+i].camera.K[4] = model->frames[i]->camera->fy; - msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx; - msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy; - - } - for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ - addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp); - } + int startsize = msg.local_poses.size(); + msg.local_poses.resize(startsize+model->relativeposes.size()); + msg.frames.resize(startsize+model->frames.size()); + msg.masks.resize(startsize+model->modelmasks.size()); + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + geometry_msgs::Pose pose1; + tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i])*rp, pose1); + geometry_msgs::Pose pose2; + tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose)*rp, pose2); + cv_bridge::CvImage rgbBridgeImage; + rgbBridgeImage.image = model->frames[i]->rgb; + rgbBridgeImage.encoding = "bgr8"; + cv_bridge::CvImage depthBridgeImage; + depthBridgeImage.image = model->frames[i]->depth; + depthBridgeImage.encoding = "mono16"; + cv_bridge::CvImage maskBridgeImage; + maskBridgeImage.image = model->modelmasks[i]->getMask(); + maskBridgeImage.encoding = "mono8"; + msg.local_poses[startsize+i] = pose1; + msg.frames[startsize+i].capture_time = ros::Time(); + msg.frames[startsize+i].pose = pose2; + msg.frames[startsize+i].frame_id = model->frames[i]->id; + msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); + msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); + msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg());//getMask() + + msg.frames[startsize+i].camera.K[0] = model->frames[i]->camera->fx; + msg.frames[startsize+i].camera.K[4] = model->frames[i]->camera->fy; + msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx; + msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy; + + } + for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ + addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp); + } } quasimodo_msgs::model getModelMSG(reglib::Model * model){ - quasimodo_msgs::model msg; - msg.model_id = model->id; - addToModelMSG(msg,model); + quasimodo_msgs::model msg; + msg.model_id = model->id; + addToModelMSG(msg,model); - return msg; + return msg; } std::vector getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms){ - std::vector toRet; - ifstream in(poses_file); - if (!in.is_open()){ - cout<<"ERROR: cannot find poses file "<> temp; - transform(j,k) = temp; - } - } - toRet.push_back(transform); - } - return toRet; + std::vector toRet; + ifstream in(poses_file); + if (!in.is_open()){ + cout<<"ERROR: cannot find poses file "<> temp; + transform(j,k) = temp; + } + } + toRet.push_back(transform); + } + return toRet; } Eigen::Matrix4d getMat(tf::StampedTransform tf){ - //Transform - geometry_msgs::TransformStamped tfstmsg; - tf::transformStampedTFToMsg (tf, tfstmsg); - geometry_msgs::Transform tfmsg = tfstmsg.transform; - geometry_msgs::Pose pose; - pose.orientation = tfmsg.rotation; - pose.position.x = tfmsg.translation.x; - pose.position.y = tfmsg.translation.y; - pose.position.z = tfmsg.translation.z; - Eigen::Affine3d epose; - tf::poseMsgToEigen(pose, epose); - return epose.matrix(); + //Transform + geometry_msgs::TransformStamped tfstmsg; + tf::transformStampedTFToMsg (tf, tfstmsg); + geometry_msgs::Transform tfmsg = tfstmsg.transform; + geometry_msgs::Pose pose; + pose.orientation = tfmsg.rotation; + pose.position.x = tfmsg.translation.x; + pose.position.y = tfmsg.translation.y; + pose.position.z = tfmsg.translation.z; + Eigen::Affine3d epose; + tf::poseMsgToEigen(pose, epose); + return epose.matrix(); } reglib::Model * load_metaroom_model(std::string sweep_xml){ - int slash_pos = sweep_xml.find_last_of("/"); - std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; - printf("folder: %s\n",sweep_folder.c_str()); + int slash_pos = sweep_xml.find_last_of("/"); + std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; + printf("folder: %s\n",sweep_folder.c_str()); - SimpleXMLParser parser; - SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); + SimpleXMLParser parser; + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); - reglib::Model * sweepmodel = 0; + reglib::Model * sweepmodel = 0; - Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); - cout << m2 << endl << endl; + Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); + cout << m2 << endl << endl; - std::vector current_room_frames; - for (size_t i=0; i current_room_frames; + for (size_t i=0; ifx = 532.158936; - cam->fy = 533.819214; - cam->cx = 310.514310; - cam->cy = 236.842039; + reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS + cam->fx = 532.158936; + cam->fy = 533.819214; + cam->cx = 310.514310; + cam->cy = 236.842039; -// cout<<"Intermediate cloud size "<points.size()<points.size()<frames.push_back(frame); - sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose); - sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); - } - } + current_room_frames.push_back(frame); + if(i == 0){ + sweepmodel = new reglib::Model(frame,fullmask); + }else{ + sweepmodel->frames.push_back(frame); + sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose); + sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); + } + } - //sweepmodel->recomputeModelPoints(); - printf("nr points: %i\n",sweepmodel->points.size()); + //sweepmodel->recomputeModelPoints(); + printf("nr points: %i\n",sweepmodel->points.size()); - return sweepmodel; + return sweepmodel; } void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ - boost::shared_ptr viewer; - if(debugg){ - viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); - viewer->addCoordinateSystem(0.01); - viewer->setBackgroundColor(1.0,1.0,1.0); - } - - reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.25); - massregmod->timeout = 1200; - massregmod->viewer = viewer; - massregmod->visualizationLvl = 0; - massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massregmod->nomask = true; - massregmod->stopval = 0.0001; - - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; - - std::vector bg_po; - std::vector bg_fr; - std::vector bg_mm; - bg->getData(bg_po, bg_fr, bg_mm); - bg->points = mu->getSuperPoints(bg_po,bg_fr,bg_mm,1,false); - - std::vector< std::vector > mod_po_vec; - std::vector< std::vector > mod_fr_vec; - std::vector< std::vector > mod_mm_vec; - - for(int j = 0; j < models.size(); j++){ - reglib::Model * mod = models[j]; - std::vector mod_po; - std::vector mod_fr; - std::vector mod_mm; - mod->getData(mod_po, mod_fr, mod_mm); - mod->points = mu->getSuperPoints(mod_po,mod_fr,mod_mm,1,false); - - mod_po_vec.push_back(mod_po); - mod_fr_vec.push_back(mod_fr); - mod_mm_vec.push_back(mod_mm); - } - - if(models.size() > 0 && bg->frames.size() > 0){ - std::vector cpmod; - - //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); - - cpmod.push_back(Eigen::Matrix4d::Identity());//,models.front()->relativeposes.front().inverse() * bg->relativeposes.front()); - massregmod->addModel(bg); - - for(int j = 0; j < models.size(); j++){ - //models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - //cpmod.push_back(bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); - //massregmod->addModel(models[j]); - - reglib::Model * mod = models[j]; -// std::vector mod_po; -// std::vector mod_fr; -// std::vector mod_mm; -// mod->getData(mod_po, mod_fr, mod_mm); - cpmod.push_back(bg_po.front().inverse() * mod_po_vec[j].front()); - massregmod->addModel(mod); - } - - reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); - - for(int j = 0; j < models.size(); j++){ - Eigen::Matrix4d change = mfrmod.poses[j+1] * cpmod[j+1].inverse(); - - for(unsigned int k = 0; k < mod_po_vec[j].size(); k++){ - mod_po_vec[j][k] = change*mod_po_vec[j][k]; - } - -// for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ -// models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; -// } - -// for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ -// models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; -// } - } - }else if(models.size() > 1){ - std::vector cpmod; - - //Eigen::Matrix4d first = Eigen::Matrix4d::Identity(); - Eigen::Matrix4d first = mod_po_vec.front().front(); - for(int j = 0; j < models.size(); j++){ -// models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); -// cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); -// massregmod->addModel(models[j]); - - reglib::Model * mod = models[j]; -// std::vector mod_po; -// std::vector mod_fr; -// std::vector mod_mm; -// mod->getData(mod_po, mod_fr, mod_mm); -// mod->points = mu->getSuperPoints(mod_po,mod_fr,mod_mm,1,false); -// if(j == 0){first = mod_po.front();} - cpmod.push_back(first.inverse() * mod_po_vec[j].front()); - massregmod->addModel(mod); - } - - reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); - for(int j = 0; j < models.size(); j++){ - Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); - - for(unsigned int k = 0; k < mod_po_vec[j].size(); k++){ - mod_po_vec[j][k] = change*mod_po_vec[j][k]; - } -// for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ -// models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; -// } - -// for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ -// models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; -// } - } - } - - delete massregmod; - - - std::vector bgcp = bg_po; - std::vector bgcf = bg_fr; - std::vector bgmask; - - for(unsigned int k = 0; k < bg_mm.size(); k++){ - bgmask.push_back(bg_mm[k]->getMask()); - } - -// for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ -// bgcp.push_back(bg->relativeposes[k]); -// bgcf.push_back(bg->frames[k]); -// bgmask.push_back(bg->modelmasks[k]->getMask()); -// } - - for(int j = 0; j < models.size(); j++){ - reglib::Model * model = models[j]; - - std::vector masks; - for(unsigned int i = 0; i < model->frames.size(); i++){ - reglib::RGBDFrame * frame = model->frames[i]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} - masks.push_back(mask); - } + boost::shared_ptr viewer; + //if(debugg){ + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); + viewer->addCoordinateSystem(0.01); + viewer->setBackgroundColor(1.0,1.0,1.0); + //} + + reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); + massregmod->timeout = 1200; + massregmod->viewer = viewer; + massregmod->visualizationLvl = 1; + massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massregmod->nomask = true; + massregmod->stopval = 0.0001; + + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + // std::vector bg_po; + // std::vector bg_fr; + // std::vector bg_mm; + // bg->getData(bg_po, bg_fr, bg_mm); + // bg->points = mu->getSuperPoints(bg_po,bg_fr,bg_mm,1,false); + + // std::vector< std::vector > mod_po_vec; + // std::vector< std::vector > mod_fr_vec; + // std::vector< std::vector > mod_mm_vec; + + // for(int j = 0; j < models.size(); j++){ + // reglib::Model * mod = models[j]; + // std::vector mod_po; + // std::vector mod_fr; + // std::vector mod_mm; + // mod->getData(mod_po, mod_fr, mod_mm); + // mod->points = mu->getSuperPoints(mod_po,mod_fr,mod_mm,1,false); + + // mod_po_vec.push_back(mod_po); + // mod_fr_vec.push_back(mod_fr); + // mod_mm_vec.push_back(mod_mm); + // printf("model[%i]->points = %i frames: %i\n",j,mod->points.size(),mod_fr.size()); + // } + + if(models.size() > 0 && bg->frames.size() > 0){ + std::vector cpmod; + + bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + + cpmod.push_back(Eigen::Matrix4d::Identity());//,models.front()->relativeposes.front().inverse() * bg->relativeposes.front()); + massregmod->addModel(bg); + printf("bg->points = %i\n",bg->points.size()); + + for(int j = 0; j < models.size(); j++){ + models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + cpmod.push_back(bg->frames.front()->pose.inverse() * models[j]->frames.front()->pose);//bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); + massregmod->addModel(models[j]); + + // reglib::Model * mod = models[j]; + //// std::vector mod_po; + //// std::vector mod_fr; + //// std::vector mod_mm; + //// mod->getData(mod_po, mod_fr, mod_mm); + // cpmod.push_back(bg_po.front().inverse() * mod_po_vec[j].front()); + // massregmod->addModel(mod); + + + } + + printf("TIME TO REGISTER\n"); + reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); + + printf("REGISTRATION DONE\n"); + for(int j = 0; j < models.size(); j++){ + Eigen::Matrix4d change = mfrmod.poses[j+1];// * cpmod[j+1].inverse(); + + // for(unsigned int k = 0; k < mod_po_vec[j].size(); k++){ + // mod_po_vec[j][k] = change*mod_po_vec[j][k]; + // } + + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + } + + for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ + models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; + } + } + }else if(models.size() > 1){ + std::vector cpmod; + + Eigen::Matrix4d first = Eigen::Matrix4d::Identity(); + //Eigen::Matrix4d first = mod_po_vec.front().front(); + for(int j = 0; j < models.size(); j++){ + models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); + massregmod->addModel(models[j]); + + // reglib::Model * mod = models[j]; + //// std::vector mod_po; + //// std::vector mod_fr; + //// std::vector mod_mm; + //// mod->getData(mod_po, mod_fr, mod_mm); + //// mod->points = mu->getSuperPoints(mod_po,mod_fr,mod_mm,1,false); + //// if(j == 0){first = mod_po.front();} + // cpmod.push_back(first.inverse() * mod_po_vec[j].front()); + // massregmod->addModel(mod); + } + + reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); + for(int j = 0; j < models.size(); j++){ + Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); + + // for(unsigned int k = 0; k < mod_po_vec[j].size(); k++){ + // mod_po_vec[j][k] = change*mod_po_vec[j][k]; + // } + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + } + + for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ + models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; + } + } + } + + delete massregmod; + + + // std::vector bgcp = bg_po; + // std::vector bgcf = bg_fr; + // std::vector bgmask; + + // for(unsigned int k = 0; k < bg_mm.size(); k++){ + // bgmask.push_back(bg_mm[k]->getMask()); + // } + + std::vector bgcp; + std::vector bgcf; + std::vector bgmask; + for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ + bgcp.push_back(bg->relativeposes[k]); + bgcf.push_back(bg->frames[k]); + bgmask.push_back(bg->modelmasks[k]->getMask()); + } + + for(int j = 0; j < models.size(); j++){ + reglib::Model * model = models[j]; + + std::vector masks; + // for(unsigned int i = 0; i < mod_fr_vec[j].size(); i++){ + // reglib::RGBDFrame * frame = mod_fr_vec[j][i]; + for(unsigned int i = 0; i < model->frames.size(); i++){ + reglib::RGBDFrame * frame = model->frames[i]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + masks.push_back(mask); + } + // for(unsigned int i = 0; i < model->frames.size(); i++){ + // reglib::RGBDFrame * frame = model->frames[i]; + // reglib::Camera * cam = frame->camera; + // cv::Mat mask; + // mask.create(cam->height,cam->width,CV_8UC1); + // unsigned char * maskdata = (unsigned char *)(mask.data); + // for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + // masks.push_back(mask); + // } //mu->computeMovingDynamicStatic(bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions - print("TODO: %s :: %i \n",__FILE__,__LINE__); -exit(0); - - std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions - std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,false);//Determine external occlusions - std::vector dynamic_masks; - for(unsigned int i = 0; i < model->frames.size(); i++){ - reglib::RGBDFrame * frame = model->frames[i]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} - - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); - unsigned char * externalmaskdata = (unsigned char *)(external_masks[i].data); - for(unsigned int k = 0; k < cam->height * cam->width;k++){ - if(externalmaskdata[k] == 0 && internalmaskdata[k] != 0 ){ - maskdata[k] = 255; - }else{ - maskdata[k] = 0; - } - } - - dynamic_masks.push_back(mask); - -// cv::imshow( "rgb", frame->rgb ); -// cv::imshow( "internal_masks", internal_masks[i] ); -// cv::imshow( "externalmask", external_masks[i] ); -// cv::imshow( "dynamic_mask", dynamic_masks[i] ); -// cv::waitKey(0); - } - - internal.push_back(internal_masks); - external.push_back(external_masks); - dynamic.push_back(dynamic_masks); - } - -/* - for(unsigned int i = 0; visualization && i < models.size(); i++){ - std::vector internal_masks = internal[i]; - std::vector external_masks = external[i]; - std::vector dynamic_masks = dynamic[i]; - reglib::Model * model = models[i]; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - for(unsigned int j = 0; j < model->frames.size(); j++){ - reglib::RGBDFrame * frame = model->frames[j]; - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); - - reglib::Camera * camera = frame->camera; - - cv::imshow( "rgb", frame->rgb ); - cv::imshow( "internal_masks", internal_masks[j] ); - cv::imshow( "externalmask", external_masks[j] ); - cv::imshow( "dynamic_mask", dynamic_masks[j] ); - cv::waitKey(100); - - - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); - unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); - unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); - - Eigen::Matrix4d p = model->relativeposes[j]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - pcl::PointXYZRGBNormal point; - point.x = m00*x + m01*y + m02*z + m03; - point.y = m10*x + m11*y + m12*z + m13; - point.z = m20*x + m21*y + m22*z + m23; - - point.b = rgbdata[3*ind+0]; - point.g = rgbdata[3*ind+1]; - point.r = rgbdata[3*ind+2]; - - if(dynamicmaskdata[ind] != 0){ - point.b = 0; - point.g = 255; - point.r = 0; - }else if(internalmaskdata[ind] == 0){ - point.b = 0; - point.g = 0; - point.r = 255; - } - - cloud->points.push_back(point); - } - } - } - } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - while(cv::waitKey(50)!='q'){viewer->spinOnce();} - } - - res.backgroundmodel = req.backgroundmodel; - - - res.masks.resize(models.size()); - for(unsigned int i = 0; i < models.size(); i++){ - for(unsigned int j = 0; j < models[i]->frames.size(); j++){ - cv_bridge::CvImage maskBridgeImage; - maskBridgeImage.image = dynamic[i][j]; - maskBridgeImage.encoding = "mono8"; - res.masks[i].images.push_back( *(maskBridgeImage.toImageMsg()) ); - } - - res.models.push_back(quasimodo_brain::getModelMSG(models[i])); - models[i]->fullDelete(); - delete models[i]; - } - models.clear(); - */ - - delete reg; - delete mu; + // mu->computeMovingDynamicStatic(bgcp,bgcf,mod_po_vec[j],mod_fr_vec[j],debugg);//Determine self occlusions + //exit(0); + // std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,mod_po_vec[j],mod_fr_vec[j],masks,mod_po_vec[j],mod_fr_vec[j],masks,true);//Determine self occlusions + // std::vector external_masks = mu->computeDynamicObject(mod_po_vec[j],mod_fr_vec[j],masks,bgcp,bgcf,bgmask,mod_po_vec[j],mod_fr_vec[j],masks,true);//Determine external occlusions + std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions + std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,false);//Determine external occlusions + std::vector dynamic_masks; + // for(unsigned int i = 0; i < mod_fr_vec[j].size(); i++){ + // reglib::RGBDFrame * frame = mod_fr_vec[j][i]; + for(unsigned int i = 0; i < model->frames.size(); i++){ + reglib::RGBDFrame * frame = model->frames[i]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[i].data); + for(unsigned int k = 0; k < cam->height * cam->width;k++){ + if(externalmaskdata[k] == 0 && internalmaskdata[k] != 0 ){ + maskdata[k] = 255; + }else{ + maskdata[k] = 0; + } + } + + dynamic_masks.push_back(mask); + + // cv::imshow( "rgb", frame->rgb ); + // cv::imshow( "internal_masks", internal_masks[i] ); + // cv::imshow( "externalmask", external_masks[i] ); + // cv::imshow( "dynamic_mask", dynamic_masks[i] ); + // cv::waitKey(0); + } + + internal.push_back(internal_masks); + external.push_back(external_masks); + dynamic.push_back(dynamic_masks); + } + + /* + for(unsigned int i = 0; visualization && i < models.size(); i++){ + std::vector internal_masks = internal[i]; + std::vector external_masks = external[i]; + std::vector dynamic_masks = dynamic[i]; + reglib::Model * model = models[i]; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + for(unsigned int j = 0; j < model->frames.size(); j++){ + reglib::RGBDFrame * frame = model->frames[j]; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + reglib::Camera * camera = frame->camera; + + cv::imshow( "rgb", frame->rgb ); + cv::imshow( "internal_masks", internal_masks[j] ); + cv::imshow( "externalmask", external_masks[j] ); + cv::imshow( "dynamic_mask", dynamic_masks[j] ); + cv::waitKey(100); + + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); + unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); + + Eigen::Matrix4d p = model->relativeposes[j]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + pcl::PointXYZRGBNormal point; + point.x = m00*x + m01*y + m02*z + m03; + point.y = m10*x + m11*y + m12*z + m13; + point.z = m20*x + m21*y + m22*z + m23; + + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; + + if(dynamicmaskdata[ind] != 0){ + point.b = 0; + point.g = 255; + point.r = 0; + }else if(internalmaskdata[ind] == 0){ + point.b = 0; + point.g = 0; + point.r = 255; + } + + cloud->points.push_back(point); + } + } + } + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + while(cv::waitKey(50)!='q'){viewer->spinOnce();} + } + + res.backgroundmodel = req.backgroundmodel; + + + res.masks.resize(models.size()); + for(unsigned int i = 0; i < models.size(); i++){ + for(unsigned int j = 0; j < models[i]->frames.size(); j++){ + cv_bridge::CvImage maskBridgeImage; + maskBridgeImage.image = dynamic[i][j]; + maskBridgeImage.encoding = "mono8"; + res.masks[i].images.push_back( *(maskBridgeImage.toImageMsg()) ); + } + + res.models.push_back(quasimodo_brain::getModelMSG(models[i])); + models[i]->fullDelete(); + delete models[i]; + } + models.clear(); + */ + + delete reg; + delete mu; } } diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 293ed48a..53c4c0e4 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -221,7 +221,7 @@ void processMetaroom(std::string path){ bgmassreg->viewer = viewer; bgmassreg->use_surface = true; bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = visualization_lvl; + bgmassreg->visualizationLvl = 0;//visualization_lvl; bgmassreg->maskstep = 15; bgmassreg->nomaskstep = 15; bgmassreg->nomask = true; @@ -230,6 +230,44 @@ void processMetaroom(std::string path){ bgmassreg->setData(frames,masks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); + reglib::Model * av = new reglib::Model(); + av->frames = frames; + av->modelmasks = masks; + for(unsigned int i = 0; i < frames.size(); i++){ + av->relativeposes.push_back(bgmfr.poses[1].inverse()*bgmfr.poses[i+1]); + } + + + av->points = mu->getSuperPoints(av->relativeposes,av->frames,av->modelmasks,1,false); + + for(unsigned int i = 0; i < frames.size(); i++){ + frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; + } + + +// reglib::Model * fullmodel = new reglib::Model(); +// fullmodel->submodels.push_back(sweep); +// fullmodel->submodels_relativeposes.push_back(bgmfr.poses[0]); +// if(frames.size() > 0){ +// fullmodel->submodels.push_back(av); +// fullmodel->submodels_relativeposes.push_back(bgmfr.poses[1]); +// } + + reglib::Model * fullmodel = new reglib::Model(); + fullmodel->frames = sweep->frames; + fullmodel->relativeposes = sweep->relativeposes; + fullmodel->modelmasks = sweep->modelmasks; + for(unsigned int i = 0; i < frames.size(); i++){ + fullmodel->frames.push_back(frames[i]); + fullmodel->modelmasks.push_back(masks[i]); + fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); + } + + std::vector po; + std::vector fr; + std::vector mm; + fullmodel->getData(po, fr, mm); + fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); SimpleXMLParser parser; SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); @@ -254,22 +292,26 @@ void processMetaroom(std::string path){ } } + + if(prevind != -1){ std::string prev = sweep_xmls[prevind]; printf("prev: %s\n",prev.c_str()); reglib::Model * bg = quasimodo_brain::load_metaroom_model(prev); - bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); std::vector< reglib::Model * > models; - models.push_back(sweep); + //models.push_back(sweep); + //models.push_back(fullmodel); + models.push_back(av); std::vector< std::vector< cv::Mat > > internal; std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; - quasimodo_brain::segment(bg,models,internal,external,dynamic); + quasimodo_brain::segment(bg,models,internal,external,dynamic,true); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 80b7ca56..1073727c 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -39,128 +39,128 @@ namespace reglib { double mysign(double v){ - if(v < 0){return -1;} - return 1; + if(v < 0){return -1;} + return 1; } double getTime(){ - struct timeval start1; - gettimeofday(&start1, NULL); - return double(start1.tv_sec+(start1.tv_usec/1000000.0)); + struct timeval start1; + gettimeofday(&start1, NULL); + return double(start1.tv_sec+(start1.tv_usec/1000000.0)); } float graph_cut(vector& graphs_out,vector>& second_graphinds, Graph& graph_in, std::vector graph_inds){ - using adjacency_iterator = boost::graph_traits::adjacency_iterator; - typename boost::property_map::type vertex_id = boost::get(boost::vertex_index, graph_in); - typename boost::property_map::type edge_id = boost::get(boost::edge_weight, graph_in); - typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, graph_in); - - BOOST_AUTO(parities, boost::make_one_bit_color_map(boost::num_vertices(graph_in), boost::get(boost::vertex_index, graph_in))); - - float w = boost::stoer_wagner_min_cut(graph_in, boost::get(boost::edge_weight, graph_in), boost::parity_map(parities)); - - unordered_map mappings; - VertexIndex counters[2] = {0, 0}; - - graphs_out.push_back(new Graph(1)); - graphs_out.push_back(new Graph(1)); - second_graphinds.push_back(vector()); - second_graphinds.push_back(vector()); - //std::cout << "One set of vertices consists of:" << std::endl; - bool flag; - Edge edge; - for (size_t i = 0; i < boost::num_vertices(graph_in); ++i) { - int first = boost::get(parities, i); - second_graphinds[first].push_back(graph_inds[i]); - // iterate adjacent edges - adjacency_iterator ai, ai_end; - for (tie(ai, ai_end) = boost::adjacent_vertices(i, graph_in); ai != ai_end; ++ai) { - VertexIndex neighbor_index = boost::get(vertex_id, *ai); - int second = boost::get(parities, neighbor_index); - if (first == second && neighbor_index < i) { - tie(edge, flag) = boost::edge(i, neighbor_index, graph_in); - edge_weight_property weight = boost::get(edge_id, edge); - if (mappings.count(i) == 0) { - mappings[i] = counters[first]++; - } - if (mappings.count(neighbor_index) == 0) { - mappings[neighbor_index] = counters[first]++; - } - tie(edge, flag) = boost::add_edge(mappings[neighbor_index], mappings[i], weight, *graphs_out[first]); - - typename boost::property_map::type vertex_name_first = boost::get(boost::vertex_name, *graphs_out[first]); - boost::get(vertex_name_first, mappings[i]) = boost::get(vertex_name, i); - boost::get(vertex_name_first, mappings[neighbor_index]) = boost::get(vertex_name, *ai); - } - } - } - return w; + using adjacency_iterator = boost::graph_traits::adjacency_iterator; + typename boost::property_map::type vertex_id = boost::get(boost::vertex_index, graph_in); + typename boost::property_map::type edge_id = boost::get(boost::edge_weight, graph_in); + typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, graph_in); + + BOOST_AUTO(parities, boost::make_one_bit_color_map(boost::num_vertices(graph_in), boost::get(boost::vertex_index, graph_in))); + + float w = boost::stoer_wagner_min_cut(graph_in, boost::get(boost::edge_weight, graph_in), boost::parity_map(parities)); + + unordered_map mappings; + VertexIndex counters[2] = {0, 0}; + + graphs_out.push_back(new Graph(1)); + graphs_out.push_back(new Graph(1)); + second_graphinds.push_back(vector()); + second_graphinds.push_back(vector()); + //std::cout << "One set of vertices consists of:" << std::endl; + bool flag; + Edge edge; + for (size_t i = 0; i < boost::num_vertices(graph_in); ++i) { + int first = boost::get(parities, i); + second_graphinds[first].push_back(graph_inds[i]); + // iterate adjacent edges + adjacency_iterator ai, ai_end; + for (tie(ai, ai_end) = boost::adjacent_vertices(i, graph_in); ai != ai_end; ++ai) { + VertexIndex neighbor_index = boost::get(vertex_id, *ai); + int second = boost::get(parities, neighbor_index); + if (first == second && neighbor_index < i) { + tie(edge, flag) = boost::edge(i, neighbor_index, graph_in); + edge_weight_property weight = boost::get(edge_id, edge); + if (mappings.count(i) == 0) { + mappings[i] = counters[first]++; + } + if (mappings.count(neighbor_index) == 0) { + mappings[neighbor_index] = counters[first]++; + } + tie(edge, flag) = boost::add_edge(mappings[neighbor_index], mappings[i], weight, *graphs_out[first]); + + typename boost::property_map::type vertex_name_first = boost::get(boost::vertex_name, *graphs_out[first]); + boost::get(vertex_name_first, mappings[i]) = boost::get(vertex_name, i); + boost::get(vertex_name_first, mappings[neighbor_index]) = boost::get(vertex_name, *ai); + } + } + } + return w; } float recursive_split(std::vector * graphs_out,std::vector> * graphinds_out, Graph * graph, std::vector graph_inds){ - if(boost::num_vertices(*graph) == 1){ - graphs_out->push_back(graph); - graphinds_out->push_back(graph_inds); - return 0; - } - - vector second_graphs; - vector> second_graphinds; - float w = graph_cut(second_graphs,second_graphinds,*graph,graph_inds); - if(w <= 0){ - delete graph; - return 2*w + recursive_split(graphs_out, graphinds_out,second_graphs.front(),second_graphinds.front()) + recursive_split(graphs_out, graphinds_out, second_graphs.back(),second_graphinds.back()); - }else{ - graphs_out->push_back(graph); - graphinds_out->push_back(graph_inds); - delete second_graphs.front(); - delete second_graphs.back(); - return 0; - } + if(boost::num_vertices(*graph) == 1){ + graphs_out->push_back(graph); + graphinds_out->push_back(graph_inds); + return 0; + } + + vector second_graphs; + vector> second_graphinds; + float w = graph_cut(second_graphs,second_graphinds,*graph,graph_inds); + if(w <= 0){ + delete graph; + return 2*w + recursive_split(graphs_out, graphinds_out,second_graphs.front(),second_graphinds.front()) + recursive_split(graphs_out, graphinds_out, second_graphs.back(),second_graphinds.back()); + }else{ + graphs_out->push_back(graph); + graphinds_out->push_back(graph_inds); + delete second_graphs.front(); + delete second_graphs.back(); + return 0; + } } std::vector partition_graph(std::vector< std::vector< float > > & scores){ - int nr_data = scores.size(); - Graph* graph = new Graph(nr_data); - std::vector graph_inds; - graph_inds.resize(nr_data); - - typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, *graph); - - float sum = 0; - for(int i = 0; i < nr_data; i++){ - graph_inds[i] = i; - for(int j = i+1; j < nr_data; j++){ - float weight = scores[i][j]; - if(weight != 0){ - sum += 2*weight; - edge_weight_property e = weight; - boost::add_edge(i, j, e, *graph); - } - } - } - - std::vector * graphs_out = new std::vector(); - std::vector> * graphinds_out = new std::vector>(); - float best = sum-recursive_split(graphs_out,graphinds_out, graph,graph_inds ); - - std::vector part; - part.resize(nr_data); - //int * part = new int[nr_data]; - for(unsigned int i = 0; i < graphinds_out->size(); i++){ - for(unsigned int j = 0; j < graphinds_out->at(i).size(); j++){ - part[graphinds_out->at(i).at(j)] = i; - } - } - return part; + int nr_data = scores.size(); + Graph* graph = new Graph(nr_data); + std::vector graph_inds; + graph_inds.resize(nr_data); + + typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, *graph); + + float sum = 0; + for(int i = 0; i < nr_data; i++){ + graph_inds[i] = i; + for(int j = i+1; j < nr_data; j++){ + float weight = scores[i][j]; + if(weight != 0){ + sum += 2*weight; + edge_weight_property e = weight; + boost::add_edge(i, j, e, *graph); + } + } + } + + std::vector * graphs_out = new std::vector(); + std::vector> * graphinds_out = new std::vector>(); + float best = sum-recursive_split(graphs_out,graphinds_out, graph,graph_inds ); + + std::vector part; + part.resize(nr_data); + //int * part = new int[nr_data]; + for(unsigned int i = 0; i < graphinds_out->size(); i++){ + for(unsigned int j = 0; j < graphinds_out->at(i).size(); j++){ + part[graphinds_out->at(i).at(j)] = i; + } + } + return part; } std::vector ModelUpdater::getPartition(std::vector< std::vector< float > > & scores, int dims, int nr_todo, double timelimit){ - return partition_graph(scores); + return partition_graph(scores); } ModelUpdater::ModelUpdater(){ - occlusion_penalty = 10; + occlusion_penalty = 10; massreg_timeout = 60; model = 0; show_init_lvl = 0;//init show @@ -181,481 +181,481 @@ ModelUpdater::~ModelUpdater(){} FusionResults ModelUpdater::registerModel(Model * model2, Eigen::Matrix4d guess, double uncertanity){return FusionResults();} void ModelUpdater::fuse(Model * model2, Eigen::Matrix4d guess, double uncertanity){ - for(unsigned int i = 0; i < model2->frames.size();i++){ - model->frames.push_back(model2->frames[i]); - //model->masks.push_back(model2->masks[i]); - model->modelmasks.push_back(model2->modelmasks[i]); - model->relativeposes.push_back(guess*model2->relativeposes[i]); - } + for(unsigned int i = 0; i < model2->frames.size();i++){ + model->frames.push_back(model2->frames[i]); + //model->masks.push_back(model2->masks[i]); + model->modelmasks.push_back(model2->modelmasks[i]); + model->relativeposes.push_back(guess*model2->relativeposes[i]); + } } UpdatedModels ModelUpdater::fuseData(FusionResults * f, Model * model1,Model * model2){return UpdatedModels();} bool ModelUpdater::isRefinementNeeded(){ - vector > occlusionScores; - occlusionScores.resize(model->frames.size()); - for(unsigned int i = 0; i < model->frames.size(); i++){occlusionScores[i].resize(model->frames.size());} - - - for(unsigned int i = 0; i < model->frames.size(); i++){ - for(unsigned int j = i+1; j < model->frames.size(); j++){ - Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; - occlusionScores[j][i] = computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,false); - occlusionScores[i][j] = computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,false); - } - } - - - std::vector > scores = getScores(occlusionScores); - std::vector partition = getPartition(scores,2,5,2); - - bool failed = false; - for(unsigned int i = 0; i < partition.size(); i++){failed = failed | (partition[i]!=0);} - - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - if(scores[i][j] > 0){printf(" ");} - printf("%5.5f ",0.0001*scores[i][j]); -// if(scores[i][j] < 0){ -// Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; -// computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,true); -// computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,true); -// } - } - printf("\n"); - } - - printf("partition"); - for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} - printf("\n"); - return failed; + vector > occlusionScores; + occlusionScores.resize(model->frames.size()); + for(unsigned int i = 0; i < model->frames.size(); i++){occlusionScores[i].resize(model->frames.size());} + + + for(unsigned int i = 0; i < model->frames.size(); i++){ + for(unsigned int j = i+1; j < model->frames.size(); j++){ + Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; + occlusionScores[j][i] = computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,false); + occlusionScores[i][j] = computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,false); + } + } + + + std::vector > scores = getScores(occlusionScores); + std::vector partition = getPartition(scores,2,5,2); + + bool failed = false; + for(unsigned int i = 0; i < partition.size(); i++){failed = failed | (partition[i]!=0);} + + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + if(scores[i][j] > 0){printf(" ");} + printf("%5.5f ",0.0001*scores[i][j]); + // if(scores[i][j] < 0){ + // Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; + // computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,true); + // computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,true); + // } + } + printf("\n"); + } + + printf("partition"); + for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} + printf("\n"); + return failed; } OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step, bool debugg){ -// printf("start:: %s::%i\n",__FILE__,__LINE__); - OcclusionScore oc; - - unsigned char * dst_maskdata = (unsigned char *)(cm->mask.data); - unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); - float * dst_normalsdata = (float *)(cf->normals.data); - - float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); - float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); - float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); - - Camera * dst_camera = cf->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - - int nr_data = spvec.size(); - - std::vector residuals; - std::vector debugg_src_inds; - std::vector debugg_dst_inds; - std::vector weights; - residuals.reserve(nr_data); - if(debugg){ - debugg_src_inds.reserve(nr_data); - debugg_dst_inds.reserve(nr_data); - } - weights.reserve(nr_data); - - for(unsigned int ind = 0; ind < spvec.size();ind+=step){ - superpoint & sp = spvec[ind]; - - float src_x = sp.point(0); - float src_y = sp.point(1); - float src_z = sp.point(2); - - float src_nx = sp.normal(0); - float src_ny = sp.normal(1); - float src_nz = sp.normal(2); - - float point_information = sp.point_information; - - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - - double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - double dst_noise = dst_z * dst_z; - double point_noise = 1.0/sqrt(point_information); - - double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); - d *= compare_mul; - - double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); - double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); - - residuals.push_back(d); - weights.push_back(angle_dst*angle_dst*angle_dst); - if(debugg){ - debugg_src_inds.push_back(ind); - debugg_dst_inds.push_back(dst_ind); - } - } - } - } - -// DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); -// func->maxp = 1.0; -// func->update_size = true; -// func->zeromean = true; -// func->startreg = 0.0001; -// func->debugg_print = debugg; -// func->bidir = true; -// func->maxnoise = pred; -// func->reset(); - - DistanceWeightFunction2 * func = new DistanceWeightFunction2(); - func->f = THRESHOLD; + // printf("start:: %s::%i\n",__FILE__,__LINE__); + OcclusionScore oc; + + unsigned char * dst_maskdata = (unsigned char *)(cm->mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); + float * dst_normalsdata = (float *)(cf->normals.data); + + float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); + float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); + float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); + + Camera * dst_camera = cf->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + int nr_data = spvec.size(); + + std::vector residuals; + std::vector debugg_src_inds; + std::vector debugg_dst_inds; + std::vector weights; + residuals.reserve(nr_data); + if(debugg){ + debugg_src_inds.reserve(nr_data); + debugg_dst_inds.reserve(nr_data); + } + weights.reserve(nr_data); + + for(unsigned int ind = 0; ind < spvec.size();ind+=step){ + superpoint & sp = spvec[ind]; + + float src_x = sp.point(0); + float src_y = sp.point(1); + float src_z = sp.point(2); + + float src_nx = sp.normal(0); + float src_ny = sp.normal(1); + float src_nz = sp.normal(2); + + float point_information = sp.point_information; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + double dst_noise = dst_z * dst_z; + double point_noise = 1.0/sqrt(point_information); + + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; + + double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); + double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); + + residuals.push_back(d); + weights.push_back(angle_dst*angle_dst*angle_dst); + if(debugg){ + debugg_src_inds.push_back(ind); + debugg_dst_inds.push_back(dst_ind); + } + } + } + } + + // DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + // func->maxp = 1.0; + // func->update_size = true; + // func->zeromean = true; + // func->startreg = 0.0001; + // func->debugg_print = debugg; + // func->bidir = true; + // func->maxnoise = pred; + // func->reset(); + + DistanceWeightFunction2 * func = new DistanceWeightFunction2(); + func->f = THRESHOLD; func->p = 0.01; - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); - for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} - func->computeModel(X); - - Eigen::VectorXd W = func->getProbs(X); - - delete func; - - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - oc.score += weight*weights.at(i); - oc.occlusions += ocl*weights.at(i); - } - - - if(debugg){ - //cf->show(true); - - pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); - dcloud->points.resize(dst_width*dst_height); - for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - unsigned int dst_ind = dst_h*dst_width+dst_w; - float z = dst_idepth*float(dst_depthdata[dst_ind]); - if(z > 0){ - float x = (float(dst_w) - dst_cx) * z * dst_ifx; - float y = (float(dst_h) - dst_cy) * z * dst_ify; - dcloud->points[dst_ind].x = x; - dcloud->points[dst_ind].y = y; - dcloud->points[dst_ind].z = z; - dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; - dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; - dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; - if(dst_w % 10 == 0 && dst_h % 10 == 0){ - //printf("%i %i -> %4.4f %4.4f %4.4f\n",dst_w,dst_h,dst_normalsdata[3*dst_ind+0],dst_h,dst_normalsdata[3*dst_ind+1],dst_h,dst_normalsdata[3*dst_ind+2]); - } - //if(false && dst_maskdata[dst_ind] == 255){ - if(dst_maskdata[dst_ind] == 255){ - dcloud->points[dst_ind].r = 255; - dcloud->points[dst_ind].g = 000; - dcloud->points[dst_ind].b = 255; - } - } - } - } - - oc.print(); - pcl::PointCloud::Ptr scloud (new pcl::PointCloud); - for(unsigned int i = 0; i < spvec.size(); i++){ - superpoint & sp = spvec[i]; - pcl::PointXYZRGBNormal p; - p.x = sp.point(0); - p.y = sp.point(1); - p.z = sp.point(2); - - float tx = m00*p.x + m01*p.y + m02*p.z + m03; - float ty = m10*p.x + m11*p.y + m12*p.z + m13; - float tz = m20*p.x + m21*p.y + m22*p.z + m23; - - p.x = tx; - p.y = ty; - p.z = tz; - - p.normal_x = sp.normal(0); - p.normal_y = sp.normal(1); - p.normal_z = sp.normal(2); - p.r = sp.feature(0); - p.g = sp.feature(1); - p.b = sp.feature(2); - - scloud->points.push_back(p); - } - -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); -// viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); -// viewer->spin(); - - viewer->removeAllPointClouds(); - viewer->removeAllShapes(); - - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - if(debugg){ - unsigned int src_ind = debugg_src_inds[i]; - unsigned int dst_ind = debugg_dst_inds[i]; - if(ocl > 0.01 || weight > 0.01){ - scloud->points[src_ind].r = 255.0*ocl*weights.at(i); - scloud->points[src_ind].g = 255.0*weight*weights.at(i); - scloud->points[src_ind].b = 0; + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); + for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} + func->computeModel(X); + + Eigen::VectorXd W = func->getProbs(X); + + delete func; + + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + oc.score += weight*weights.at(i); + oc.occlusions += ocl*weights.at(i); + } + + + if(debugg){ + //cf->show(true); + + pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + dcloud->points.resize(dst_width*dst_height); + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + unsigned int dst_ind = dst_h*dst_width+dst_w; + float z = dst_idepth*float(dst_depthdata[dst_ind]); + if(z > 0){ + float x = (float(dst_w) - dst_cx) * z * dst_ifx; + float y = (float(dst_h) - dst_cy) * z * dst_ify; + dcloud->points[dst_ind].x = x; + dcloud->points[dst_ind].y = y; + dcloud->points[dst_ind].z = z; + dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; + dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; + dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; + if(dst_w % 10 == 0 && dst_h % 10 == 0){ + //printf("%i %i -> %4.4f %4.4f %4.4f\n",dst_w,dst_h,dst_normalsdata[3*dst_ind+0],dst_h,dst_normalsdata[3*dst_ind+1],dst_h,dst_normalsdata[3*dst_ind+2]); + } + //if(false && dst_maskdata[dst_ind] == 255){ + if(dst_maskdata[dst_ind] == 255){ + dcloud->points[dst_ind].r = 255; + dcloud->points[dst_ind].g = 000; + dcloud->points[dst_ind].b = 255; + } + } + } + } + + oc.print(); + pcl::PointCloud::Ptr scloud (new pcl::PointCloud); + for(unsigned int i = 0; i < spvec.size(); i++){ + superpoint & sp = spvec[i]; + pcl::PointXYZRGBNormal p; + p.x = sp.point(0); + p.y = sp.point(1); + p.z = sp.point(2); + + float tx = m00*p.x + m01*p.y + m02*p.z + m03; + float ty = m10*p.x + m11*p.y + m12*p.z + m13; + float tz = m20*p.x + m21*p.y + m22*p.z + m23; + + p.x = tx; + p.y = ty; + p.z = tz; + + p.normal_x = sp.normal(0); + p.normal_y = sp.normal(1); + p.normal_z = sp.normal(2); + p.r = sp.feature(0); + p.g = sp.feature(1); + p.b = sp.feature(2); + + scloud->points.push_back(p); + } + + // viewer->removeAllPointClouds(); + // viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + // viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + // viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->removeAllShapes(); + + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + unsigned int src_ind = debugg_src_inds[i]; + unsigned int dst_ind = debugg_dst_inds[i]; + if(ocl > 0.01 || weight > 0.01){ + scloud->points[src_ind].r = 255.0*ocl*weights.at(i); + scloud->points[src_ind].g = 255.0*weight*weights.at(i); + scloud->points[src_ind].b = 0; if(i % 300 == 0){ - char buf [1024]; - sprintf(buf,"line%i",i); - viewer->addLine (scloud->points[src_ind], dcloud->points[dst_ind],buf); - } - }else{ - scloud->points[src_ind].x = 0; - scloud->points[src_ind].y = 0; - scloud->points[src_ind].z = 0; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud"); - - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - viewer->spin(); - viewer->removeAllPointClouds(); - viewer->removeAllShapes(); - - } -//printf("stop :: %s::%i\n",__FILE__,__LINE__); - return oc; + char buf [1024]; + sprintf(buf,"line%i",i); + viewer->addLine (scloud->points[src_ind], dcloud->points[dst_ind],buf); + } + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud"); + + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + viewer->removeAllShapes(); + + } + //printf("stop :: %s::%i\n",__FILE__,__LINE__); + return oc; } OcclusionScore ModelUpdater::computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp, int step, bool debugg){ - OcclusionScore ocs; - for(unsigned int i = 0; i < cp.size(); i++){ - ocs.add(computeOcclusionScore(mod->points,cp[i].inverse()*rp,cf[i],cm[i],step,debugg)); - } - return ocs; + OcclusionScore ocs; + for(unsigned int i = 0; i < cp.size(); i++){ + ocs.add(computeOcclusionScore(mod->points,cp[i].inverse()*rp,cf[i],cm[i],step,debugg)); + } + return ocs; } OcclusionScore ModelUpdater::computeOcclusionScore(Model * model1, Model * model2, Matrix4d rp, int step, bool debugg){ - OcclusionScore ocs; -// ocs.add(computeOcclusionScore(model1, model2->rep_relativeposes,model2->rep_frames,model2->rep_modelmasks,rp.inverse(),step,debugg)); -// ocs.add(computeOcclusionScore(model2, model1->rep_relativeposes,model1->rep_frames,model1->rep_modelmasks,rp,step,debugg)); - ocs.add(computeOcclusionScore(model1, model2->relativeposes,model2->frames,model2->modelmasks,rp.inverse(),step,debugg)); - ocs.add(computeOcclusionScore(model2, model1->relativeposes,model1->frames,model1->modelmasks,rp,step,debugg)); - return ocs; + OcclusionScore ocs; + // ocs.add(computeOcclusionScore(model1, model2->rep_relativeposes,model2->rep_frames,model2->rep_modelmasks,rp.inverse(),step,debugg)); + // ocs.add(computeOcclusionScore(model2, model1->rep_relativeposes,model1->rep_frames,model1->rep_modelmasks,rp,step,debugg)); + ocs.add(computeOcclusionScore(model1, model2->relativeposes,model2->frames,model2->modelmasks,rp.inverse(),step,debugg)); + ocs.add(computeOcclusionScore(model2, model1->relativeposes,model1->frames,model1->modelmasks,rp,step,debugg)); + return ocs; } vector > ModelUpdater::computeOcclusionScore(vector models, vector rps, int step, bool debugg){ - unsigned int nr_models = models.size(); - vector > occlusionScores; - occlusionScores.resize(nr_models); - for(unsigned int i = 0; i < nr_models; i++){occlusionScores[i].resize(nr_models);} - - for(unsigned int i = 0; i < nr_models; i++){ - for(unsigned int j = i+1; j < nr_models; j++){ - //printf("%5.5f %5.5f\n",double(i+1)/double(nr_models),double(j+1)/double(nr_models)); - Eigen::Matrix4d relative_pose = rps[i].inverse() * rps[j]; - occlusionScores[i][j] = computeOcclusionScore(models[i],models[j], relative_pose, step, debugg); - occlusionScores[j][i] = occlusionScores[i][j]; - } - } - - return occlusionScores; + unsigned int nr_models = models.size(); + vector > occlusionScores; + occlusionScores.resize(nr_models); + for(unsigned int i = 0; i < nr_models; i++){occlusionScores[i].resize(nr_models);} + + for(unsigned int i = 0; i < nr_models; i++){ + for(unsigned int j = i+1; j < nr_models; j++){ + //printf("%5.5f %5.5f\n",double(i+1)/double(nr_models),double(j+1)/double(nr_models)); + Eigen::Matrix4d relative_pose = rps[i].inverse() * rps[j]; + occlusionScores[i][j] = computeOcclusionScore(models[i],models[j], relative_pose, step, debugg); + occlusionScores[j][i] = occlusionScores[i][j]; + } + } + + return occlusionScores; } double ModelUpdater::computeOcclusionScoreCosts(vector models){ - double total = 0; + double total = 0; - unsigned int nr_models = models.size(); - for(unsigned int i = 0; i < nr_models; i++){ - for(unsigned int j = 0; j < nr_models; j++){ - if(i == j){continue;} - total += models[j]->rep_frames.size() * models[i]->points.size(); - } - } + unsigned int nr_models = models.size(); + for(unsigned int i = 0; i < nr_models; i++){ + for(unsigned int j = 0; j < nr_models; j++){ + if(i == j){continue;} + total += models[j]->rep_frames.size() * models[i]->points.size(); + } + } - return total; + return total; } void ModelUpdater::addModelsToVector(vector & models, vector & rps, Model * model, Matrix4d rp){ - if(model->frames.size() > 0){ - models.push_back(model); - rps.push_back(rp); - } - - for(unsigned int i = 0; i < model->submodels.size(); i++){ - addModelsToVector(models,rps,model->submodels[i], rp*model->submodels_relativeposes[i]); - } + if(model->frames.size() > 0){ + models.push_back(model); + rps.push_back(rp); + } + + for(unsigned int i = 0; i < model->submodels.size(); i++){ + addModelsToVector(models,rps,model->submodels[i], rp*model->submodels_relativeposes[i]); + } } pcl::PointCloud::Ptr ModelUpdater::getPCLnormalcloud(vector & points){ - pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); - for(unsigned int i = 0; i < points.size(); i++){ - superpoint & sp = points[i]; - pcl::PointXYZRGBNormal p; - p.x = sp.point(0); - p.y = sp.point(1); - p.z = sp.point(2); - - p.normal_x = sp.normal(0); - p.normal_y = sp.normal(1); - p.normal_z = sp.normal(2); - p.r = sp.feature(0); - p.g = sp.feature(1); - p.b = sp.feature(2); - - cloud_ptr->points.push_back(p); - } - return cloud_ptr; + pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); + for(unsigned int i = 0; i < points.size(); i++){ + superpoint & sp = points[i]; + pcl::PointXYZRGBNormal p; + p.x = sp.point(0); + p.y = sp.point(1); + p.z = sp.point(2); + + p.normal_x = sp.normal(0); + p.normal_y = sp.normal(1); + p.normal_z = sp.normal(2); + p.r = sp.feature(0); + p.g = sp.feature(1); + p.b = sp.feature(2); + + cloud_ptr->points.push_back(p); + } + return cloud_ptr; } bool ModelUpdater::refineIfNeeded(){ - if(isRefinementNeeded()){ - MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1); - massreg->timeout = massreg_timeout; - massreg->viewer = viewer; - massreg->visualizationLvl = 1; - massreg->maskstep = 4; - massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); - - massreg->nomask = true; - massreg->stopval = 0.001; - - massreg->setData(model->frames,model->modelmasks); - MassFusionResults mfr = massreg->getTransforms(model->relativeposes); - model->relativeposes = mfr.poses; - - isRefinementNeeded(); - } - - - MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1); - massreg->timeout = massreg_timeout; - massreg->viewer = viewer; - massreg->visualizationLvl = 1; - massreg->maskstep = 4; - massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); - - massreg->nomask = true; - massreg->stopval = 0.001; - - massreg->setData(model->frames,model->modelmasks); - MassFusionResults mfr = massreg->getTransforms(model->relativeposes); - model->relativeposes = mfr.poses; - - vector spvec = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); - vector cp; - vector cf; - vector cm; - for(unsigned int i = 0; i < model->relativeposes.size(); i++){ - cp.push_back(model->relativeposes[i]); - cf.push_back(model->frames[i]); - cm.push_back(model->modelmasks[i]); - } - getGoodCompareFrames(cp,cf,cm); - - vector spvec2 = getSuperPoints(cp,cf,cm,1,true); - return true; + if(isRefinementNeeded()){ + MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1); + massreg->timeout = massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = 1; + massreg->maskstep = 4; + massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); + + massreg->nomask = true; + massreg->stopval = 0.001; + + massreg->setData(model->frames,model->modelmasks); + MassFusionResults mfr = massreg->getTransforms(model->relativeposes); + model->relativeposes = mfr.poses; + + isRefinementNeeded(); + } + + + MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1); + massreg->timeout = massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = 1; + massreg->maskstep = 4; + massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); + + massreg->nomask = true; + massreg->stopval = 0.001; + + massreg->setData(model->frames,model->modelmasks); + MassFusionResults mfr = massreg->getTransforms(model->relativeposes); + model->relativeposes = mfr.poses; + + vector spvec = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); + vector cp; + vector cf; + vector cm; + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + cp.push_back(model->relativeposes[i]); + cf.push_back(model->frames[i]); + cm.push_back(model->modelmasks[i]); + } + getGoodCompareFrames(cp,cf,cm); + + vector spvec2 = getSuperPoints(cp,cf,cm,1,true); + return true; } void ModelUpdater::makeInitialSetup(){ - if(model->relativeposes.size() <= 1){ - model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); - - vector cp; - vector cf; - vector cm; - for(unsigned int i = 0; i < model->relativeposes.size(); i++){ - cp.push_back(model->relativeposes[i]); - cf.push_back(model->frames[i]); - cm.push_back(model->modelmasks[i]); - } - getGoodCompareFrames(cp,cf,cm); - - model->rep_relativeposes = cp; - model->rep_frames = cf; - model->rep_modelmasks = cm; - return ; - } + if(model->relativeposes.size() <= 1){ + model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); + + vector cp; + vector cf; + vector cm; + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + cp.push_back(model->relativeposes[i]); + cf.push_back(model->frames[i]); + cm.push_back(model->modelmasks[i]); + } + getGoodCompareFrames(cp,cf,cm); + + model->rep_relativeposes = cp; + model->rep_frames = cf; + model->rep_modelmasks = cm; + return ; + } int minHessian = 400; -// cv::SurfFeatureDetector detector( minHessian ); -// cv::SurfDescriptorExtractor extractor; + // cv::SurfFeatureDetector detector( minHessian ); + // cv::SurfDescriptorExtractor extractor; //cv::ORB orb = cv::ORB(250);//,1.2f, 1, 3, 0,2, cv::ORB::HARRIS_SCORE, 31); cv::ORB orb = cv::ORB(10);//,1.2f, 1, 3, 0,2, cv::ORB::HARRIS_SCORE, 31); -// for(unsigned int i = 0; i < model->frames.size(); i++){ -// std::vector keypoints; -// cv::Mat descriptors; -// //orb(model->frames[i]->rgb, model->modelmasks[i]->getMask(), keypoints, descriptors); -// orb(model->frames[i]->rgb, cv::Mat(), keypoints, descriptors); -// model->all_keypoints.push_back(keypoints); -// model->all_descriptors.push_back(descriptors); + // for(unsigned int i = 0; i < model->frames.size(); i++){ + // std::vector keypoints; + // cv::Mat descriptors; + // //orb(model->frames[i]->rgb, model->modelmasks[i]->getMask(), keypoints, descriptors); + // orb(model->frames[i]->rgb, cv::Mat(), keypoints, descriptors); + // model->all_keypoints.push_back(keypoints); + // model->all_descriptors.push_back(descriptors); -// printf("keypoints: %i\n",keypoints.size()); + // printf("keypoints: %i\n",keypoints.size()); -// cv::Mat descriptors_surf; -// std::vector keypoints_surf; + // cv::Mat descriptors_surf; + // std::vector keypoints_surf; -// detector.detect(model->frames[i]->rgb, keypoints_surf, model->modelmasks[i]->getMask() ); -// extractor.compute( model->frames[i]->rgb, keypoints_surf, descriptors_surf ); -// model->all_keypoints.push_back(keypoints_surf); -// model->all_descriptors.push_back(descriptors_surf); + // detector.detect(model->frames[i]->rgb, keypoints_surf, model->modelmasks[i]->getMask() ); + // extractor.compute( model->frames[i]->rgb, keypoints_surf, descriptors_surf ); + // model->all_keypoints.push_back(keypoints_surf); + // model->all_descriptors.push_back(descriptors_surf); -//// printf("keypoints: %i\n",keypoints.size()); -//// cv::Mat out; -//// cv::drawKeypoints(model->frames[i]->rgb, keypoints_surf, out, cv::Scalar::all(255)); -//// cv::imshow("Kpts", out); -//// cv::waitKey(0); -// } + //// printf("keypoints: %i\n",keypoints.size()); + //// cv::Mat out; + //// cv::drawKeypoints(model->frames[i]->rgb, keypoints_surf, out, cv::Scalar::all(255)); + //// cv::imshow("Kpts", out); + //// cv::waitKey(0); + // } -// show_refine = false;//refine show -// show_reg = false;//registration show -// show_scoring = false;//fuse scoring sho + // show_refine = false;//refine show + // show_reg = false;//registration show + // show_scoring = false;//fuse scoring sho MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); massreg->timeout = 4*massreg_timeout; @@ -673,17 +673,17 @@ void ModelUpdater::makeInitialSetup(){ /* MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); - massreg->timeout = 4*massreg_timeout; - massreg->viewer = viewer; + massreg->timeout = 4*massreg_timeout; + massreg->viewer = viewer; massreg->visualizationLvl = show_init_lvl; massreg->maskstep = std::max(1,int(0.25*double(model->frames.size()))); massreg->nomaskstep = std::max(5,int(0.5+0.5*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massreg->nomask = true; - massreg->stopval = 0.0001; + massreg->nomask = true; + massreg->stopval = 0.0001; - //massreg->setData(model->frames,model->modelmasks); - massreg->addModelData(model, false); + //massreg->setData(model->frames,model->modelmasks); + massreg->addModelData(model, false); std::vector p = model->submodels_relativeposes; p.insert(p.end(), model->relativeposes.begin(), model->relativeposes.end()); @@ -710,591 +710,591 @@ void ModelUpdater::makeInitialSetup(){ - model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); -printf("getSuperPoints done\n"); - vector cp; - vector cf; - vector cm; - for(unsigned int i = 0; i < model->relativeposes.size(); i++){ - cp.push_back(model->relativeposes[i]); - cf.push_back(model->frames[i]); - cm.push_back(model->modelmasks[i]); - } + model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); + printf("getSuperPoints done\n"); + vector cp; + vector cf; + vector cm; + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + cp.push_back(model->relativeposes[i]); + cf.push_back(model->frames[i]); + cm.push_back(model->modelmasks[i]); + } //getGoodCompareFrames(cp,cf,cm); -computeOcclusionAreas(cp,cf,cm); -exit(0); - model->rep_relativeposes = cp; - model->rep_frames = cf; - model->rep_modelmasks = cm; + computeOcclusionAreas(cp,cf,cm); + exit(0); + model->rep_relativeposes = cp; + model->rep_frames = cf; + model->rep_modelmasks = cm; model->save("latestModel"); -// vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); -// std::vector > scores2 = getScores(ocs2); + // vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); + // std::vector > scores2 = getScores(ocs2); -// double sumscore_aft = 0; -// for(unsigned int i = 0; i < scores2.size(); i++){ -// for(unsigned int j = 0; j < scores2.size(); j++){ -// sumscore_aft += scores2[i][j]; -// } -// } + // double sumscore_aft = 0; + // for(unsigned int i = 0; i < scores2.size(); i++){ + // for(unsigned int j = 0; j < scores2.size(); j++){ + // sumscore_aft += scores2[i][j]; + // } + // } -// printf("bef %f after %f\n",sumscore_bef,sumscore_aft); -// if(sumscore_aft >= sumscore_bef){ -// model->submodels_relativeposes = mfr.poses; -// } + // printf("bef %f after %f\n",sumscore_bef,sumscore_aft); + // if(sumscore_aft >= sumscore_bef){ + // model->submodels_relativeposes = mfr.poses; + // } } void ModelUpdater::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, int type, bool debugg){ - bool * maskvec = modelmask->maskvec; - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); + bool * maskvec = modelmask->maskvec; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); - unsigned int frameid = frame->id; + unsigned int frameid = frame->id; - Matrix4d ip = p.inverse(); + Matrix4d ip = p.inverse(); - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - float im00 = ip(0,0); float im01 = ip(0,1); float im02 = ip(0,2); float im03 = ip(0,3); - float im10 = ip(1,0); float im11 = ip(1,1); float im12 = ip(1,2); float im13 = ip(1,3); - float im20 = ip(2,0); float im21 = ip(2,1); float im22 = ip(2,2); float im23 = ip(2,3); + float im00 = ip(0,0); float im01 = ip(0,1); float im02 = ip(0,2); float im03 = ip(0,3); + float im10 = ip(1,0); float im11 = ip(1,1); float im12 = ip(1,2); float im13 = ip(1,3); + float im20 = ip(2,0); float im21 = ip(2,1); float im22 = ip(2,2); float im23 = ip(2,3); - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; - const unsigned int dst_width2 = camera->width - 2; - const unsigned int dst_height2 = camera->height - 2; - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float fx = camera->fx; - const float fy = camera->fy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - - //bool * isfused = new bool[width*height]; - std::vector isfused; - printf("wh: %i %i -> %i\n",width,height,width*height); - isfused.resize(width*height); - for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} - - - for(unsigned int ind = 0; ind < spvec.size();ind++){ - superpoint & sp = spvec[ind]; - - float src_x = sp.point(0); - float src_y = sp.point(1); - float src_z = sp.point(2); - - float src_nx = sp.normal(0); - float src_ny = sp.normal(1); - float src_nz = sp.normal(2); - - float src_r = sp.feature(0); - float src_g = sp.feature(1); - float src_b = sp.feature(2); - - double point_information = sp.point_information; - double feature_information = sp.feature_information; - - float tx = im00*src_x + im01*src_y + im02*src_z + im03; - float ty = im10*src_x + im11*src_y + im12*src_z + im13; - float tz = im20*src_x + im21*src_y + im22*src_z + im23; - - float itz = 1.0/tz; - float dst_w = fx*tx*itz + cx; - float dst_h = fy*ty*itz + cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * width + unsigned(dst_w+0.5); - - float dst_z = idepth*float(depthdata[dst_ind]); - float dst_nx = normalsdata[3*dst_ind+0]; - if(!isfused[dst_ind] && dst_z > 0 && dst_nx != 2){ - float dst_ny = normalsdata[3*dst_ind+1]; - float dst_nz = normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - cx) * dst_z * ifx; - float dst_y = (float(dst_h) - cy) * dst_z * ify; - - double dst_noise = dst_z * dst_z; - - double point_noise = 1.0/sqrt(point_information); - - float tnx = im00*src_nx + im01*src_ny + im02*src_nz; - float tny = im10*src_nx + im11*src_ny + im12*src_nz; - float tnz = im20*src_nx + im21*src_ny + im22*src_nz; - - double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - - double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); - d *= compare_mul; - - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - - if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE - float px = m00*dst_x + m01*dst_y + m02*dst_z + m03; - float py = m10*dst_x + m11*dst_y + m12*dst_z + m13; - float pz = m20*dst_x + m21*dst_y + m22*dst_z + m23; - - float pnx = m00*dst_nx + m01*dst_ny + m02*dst_nz; - float pny = m10*dst_nx + m11*dst_ny + m12*dst_nz; - float pnz = m20*dst_nx + m21*dst_ny + m22*dst_nz; - - float pb = rgbdata[3*dst_ind+0]; - float pg = rgbdata[3*dst_ind+1]; - float pr = rgbdata[3*dst_ind+2]; - - Vector3f pxyz (px ,py ,pz ); - Vector3f pnxyz (pnx,pny,pnz); - Vector3f prgb (pr ,pg ,pb ); - float weight = 1.0/(dst_noise*dst_noise); - superpoint sp2 = superpoint(pxyz,pnxyz,prgb, weight, weight, frameid); - sp.merge(sp2); - isfused[dst_ind] = true; - } - } - } - } - - int nr_fused = 0; - int nr_mask = 0; - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - nr_fused += isfused[ind]; - nr_mask += maskvec[ind] > 0; - } - } - - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - if(!isfused[ind] && maskvec[ind]){ - float z = idepth*float(depthdata[ind]); - float nx = normalsdata[3*ind+0]; - - if(z > 0 && nx != 2){ - float ny = normalsdata[3*ind+1]; - float nz = normalsdata[3*ind+2]; - - float x = (w - cx) * z * ifx; - float y = (h - cy) * z * ify; - - double noise = z * z; - - float px = m00*x + m01*y + m02*z + m03; - float py = m10*x + m11*y + m12*z + m13; - float pz = m20*x + m21*y + m22*z + m23; - float pnx = m00*nx + m01*ny + m02*nz; - float pny = m10*nx + m11*ny + m12*nz; - float pnz = m20*nx + m21*ny + m22*nz; - - float pb = rgbdata[3*ind+0]; - float pg = rgbdata[3*ind+1]; - float pr = rgbdata[3*ind+2]; - - Vector3f pxyz (px ,py ,pz ); - Vector3f pnxyz (pnx,pny,pnz); - Vector3f prgb (pr ,pg ,pb ); - float weight = 1.0/(noise*noise); - spvec.push_back(superpoint(pxyz,pnxyz,prgb, weight, weight, frameid)); - } - } - } - } - - //delete[] isfused; - - if(debugg){ - pcl::PointCloud::Ptr cloud = getPCLnormalcloud(spvec); - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - } -} + const unsigned int dst_width2 = camera->width - 2; + const unsigned int dst_height2 = camera->height - 2; -vector ModelUpdater::getSuperPoints(vector cp, vector cf, vector cm, int type, bool debugg){ - vector spvec; - for(unsigned int i = 0; i < cp.size(); i++){addSuperPoints(spvec, cp[i], cf[i], cm[i], type,debugg);} - return spvec; -} + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float fx = camera->fx; + const float fy = camera->fy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; -double ModelUpdater::getCompareUtility(Matrix4d p, RGBDFrame* frame, ModelMask* mask, vector & cp, vector & cf, vector & cm){ - unsigned char * src_maskdata = (unsigned char *)(mask->mask.data); - unsigned char * src_rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(frame->depth.data); - float * src_normalsdata = (float *)(frame->normals.data); - - Camera * src_camera = frame->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - - std::vector test_x; - std::vector test_y; - std::vector test_z; - std::vector test_nx; - std::vector test_ny; - std::vector test_nz; - - std::vector & testw = mask->testw; - std::vector & testh = mask->testh; - - for(unsigned int ind = 0; ind < testw.size();ind++){ - unsigned int src_w = testw[ind]; - unsigned int src_h = testh[ind]; - - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] == 255){ - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; - - test_x.push_back(src_x); - test_y.push_back(src_y); - test_z.push_back(src_z); - test_nx.push_back(src_nx); - test_ny.push_back(src_ny); - test_nz.push_back(src_nz); - } - } - } - - bool debugg = false; - unsigned int nrdata_start = test_x.size(); - - for(unsigned int c = 0; c < cp.size();c++){ - RGBDFrame* dst = cf[c]; - if(dst == frame){continue;} - ModelMask* dst_modelmask = cm[c]; - unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); - unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); - float * dst_normalsdata = (float *)(dst->normals.data); - - Matrix4d rp = cp[c].inverse()*p; - - float m00 = rp(0,0); float m01 = rp(0,1); float m02 = rp(0,2); float m03 = rp(0,3); - float m10 = rp(1,0); float m11 = rp(1,1); float m12 = rp(1,2); float m13 = rp(1,3); - float m20 = rp(2,0); float m21 = rp(2,1); float m22 = rp(2,2); float m23 = rp(2,3); - - Camera * dst_camera = dst->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; -pcl::PointCloud::Ptr cloud (new pcl::PointCloud); -if(debugg){ - viewer->removeAllPointClouds(); - - pcl::PointCloud::Ptr cloud2 = dst->getPCLcloud(); - viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud2"); -} + //bool * isfused = new bool[width*height]; + std::vector isfused; + printf("wh: %i %i -> %i\n",width,height,width*height); + isfused.resize(width*height); + for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} - for(unsigned int ind = 0; ind < test_x.size();ind++){ - float src_nx = test_nx[ind]; - float src_ny = test_ny[ind]; - float src_nz = test_nz[ind]; - - float src_x = test_x[ind]; - float src_y = test_y[ind]; - float src_z = test_z[ind]; - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - pcl::PointXYZRGBNormal point; - if(debugg){ - point.x = tx; - point.y = ty; - point.z = tz; - point.r = 255; - point.g = 0; - point.b = 0; - } - - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - double dst_noise = dst_z * dst_z; - double point_noise = src_z * src_z; - - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - - double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - - double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); - d *= compare_mul; - - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - - if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE - //if(d < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE - test_nx[ind] = test_nx.back(); test_nx.pop_back(); - test_ny[ind] = test_ny.back(); test_ny.pop_back(); - test_nz[ind] = test_nz.back(); test_nz.pop_back(); - test_x[ind] = test_x.back(); test_x.pop_back(); - test_y[ind] = test_y.back(); test_y.pop_back(); - test_z[ind] = test_z.back(); test_z.pop_back(); - ind--; -if(debugg){ - point.r = 0; - point.g = 255; - point.b = 0; -} - } - } - } -if(debugg){ - cloud->points.push_back(point); -} - } - -if(debugg){ - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); -} - } - return double(test_x.size())/double(nrdata_start); -} -void ModelUpdater::getGoodCompareFrames(vector & cp, vector & cf, vector & cm){ - while(true){ - double worst = 1; - int worst_id = -1; - for(unsigned int i = 0; i < cp.size(); i++){ - double score = getCompareUtility(cp[i], cf[i], cm[i], cp, cf, cm); - //printf("%3.3i -> %f\n",i,score); - if(score < worst){ - worst = score; - worst_id = i; - } - } - - if(worst_id < 0){break;} - if(worst < 0.1){//worst frame can to 90% be represented by other frames - cp[worst_id] = cp.back(); - cp.pop_back(); - - cf[worst_id] = cf.back(); - cf.pop_back(); - - cm[worst_id] = cm.back(); - cm.pop_back(); - //printf("removing %i\n",worst_id); - }else{break;} - } -} + for(unsigned int ind = 0; ind < spvec.size();ind++){ + superpoint & sp = spvec[ind]; -void ModelUpdater::refine(double reg,bool useFullMask, int visualization){ - //return ; - printf("void ModelUpdater::refine()\n"); + float src_x = sp.point(0); + float src_y = sp.point(1); + float src_z = sp.point(2); -printf("%s::%i\n",__FILE__,__LINE__); - vector > ocs = computeOcclusionScore(model->submodels,model->submodels_relativeposes,1,false); -printf("%s::%i\n",__FILE__,__LINE__); - std::vector > scores = getScores(ocs); -printf("%s::%i\n",__FILE__,__LINE__); + float src_nx = sp.normal(0); + float src_ny = sp.normal(1); + float src_nz = sp.normal(2); - double sumscore_bef = 0; - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - sumscore_bef += scores[i][j]; - } - } + float src_r = sp.feature(0); + float src_g = sp.feature(1); + float src_b = sp.feature(2); - MassFusionResults mfr; - MassRegistrationPPR * massreg = new MassRegistrationPPR(reg); - massreg->timeout = massreg_timeout; - massreg->viewer = viewer; - massreg->visualizationLvl = 0; - massreg->maskstep = 4;//std::max(1,int(0.5+0.02*double(model->frames.size()))); - massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); - printf("maskstep: %i nomaskstep: %i\n",massreg->maskstep,massreg->nomaskstep); - massreg->nomask = true; - massreg->stopval = 0.001; - double step = model->submodels_relativeposes.size()*model->submodels_relativeposes.size(); - step *= 0.25; - step += 0.5; - massreg->maskstep = std::max(1.5,step); - massreg->addModelData(model); - mfr = massreg->getTransforms(model->submodels_relativeposes); - vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); - std::vector > scores2 = getScores(ocs2); - - double sumscore_aft = 0; - for(unsigned int i = 0; i < scores2.size(); i++){ - for(unsigned int j = 0; j < scores2.size(); j++){ - sumscore_aft += scores2[i][j]; - } - } - - printf("bef %f after %f\n",sumscore_bef,sumscore_aft); - if(sumscore_aft >= sumscore_bef){ - model->submodels_relativeposes = mfr.poses; - } -} + double point_information = sp.point_information; + double feature_information = sp.feature_information; -void ModelUpdater::show(bool stop){ - //printf("void ModelUpdater::show()\n"); - pcl::PointCloud::Ptr cloud = model->getPCLcloud(1,false); - viewer->removeAllPointClouds(); - viewer->addPointCloud(cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - if(stop){ viewer->spin();} - else{ viewer->spinOnce();} - viewer->removeAllPointClouds(); -} + float tx = im00*src_x + im01*src_y + im02*src_z + im03; + float ty = im10*src_x + im11*src_y + im12*src_z + im13; + float tz = im20*src_x + im21*src_y + im22*src_z + im23; -//Backproject and prune occlusions -void ModelUpdater::pruneOcclusions(){} + float itz = 1.0/tz; + float dst_w = fx*tx*itz + cx; + float dst_h = fy*ty*itz + cy; -void ModelUpdater::computeResiduals(std::vector & residuals, std::vector & weights, - RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, - RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, - Eigen::Matrix4d p, bool debugg){ + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * width + unsigned(dst_w+0.5); - unsigned char * src_maskdata = (unsigned char *)(src_modelmask->mask.data); - unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(src->depth.data); - float * src_normalsdata = (float *)(src->normals.data); + float dst_z = idepth*float(depthdata[dst_ind]); + float dst_nx = normalsdata[3*dst_ind+0]; + if(!isfused[dst_ind] && dst_z > 0 && dst_nx != 2){ + float dst_ny = normalsdata[3*dst_ind+1]; + float dst_nz = normalsdata[3*dst_ind+2]; - unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); - unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); - float * dst_normalsdata = (float *)(dst->normals.data); + float dst_x = (float(dst_w) - cx) * dst_z * ifx; + float dst_y = (float(dst_h) - cy) * dst_z * ify; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + double dst_noise = dst_z * dst_z; - Camera * src_camera = src->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; + double point_noise = 1.0/sqrt(point_information); - Camera * dst_camera = dst->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; + float tnx = im00*src_nx + im01*src_ny + im02*src_nz; + float tny = im10*src_nx + im11*src_ny + im12*src_nz; + float tnz = im20*src_nx + im21*src_ny + im22*src_nz; - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; + double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - std::vector & testw = src_modelmask->testw; - std::vector & testh = src_modelmask->testh; + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; - unsigned int test_nrdata = testw.size(); - for(unsigned int ind = 0; ind < test_nrdata;ind++){ - unsigned int src_w = testw[ind]; - unsigned int src_h = testh[ind]; + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ - float z = src_idepth*float(src_depthdata[src_ind]); - float nx = src_normalsdata[3*src_ind+0]; + if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE + float px = m00*dst_x + m01*dst_y + m02*dst_z + m03; + float py = m10*dst_x + m11*dst_y + m12*dst_z + m13; + float pz = m20*dst_x + m21*dst_y + m22*dst_z + m23; - if(z > 0 && nx != 2){ - float ny = src_normalsdata[3*src_ind+1]; - float nz = src_normalsdata[3*src_ind+2]; + float pnx = m00*dst_nx + m01*dst_ny + m02*dst_nz; + float pny = m10*dst_nx + m11*dst_ny + m12*dst_nz; + float pnz = m20*dst_nx + m21*dst_ny + m22*dst_nz; - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; + float pb = rgbdata[3*dst_ind+0]; + float pg = rgbdata[3*dst_ind+1]; + float pr = rgbdata[3*dst_ind+2]; - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - //float tnx = m00*nx + m01*ny + m02*nz; - //float tny = m10*nx + m11*ny + m12*nz; - float tnz = m20*nx + m21*ny + m22*nz; + Vector3f pxyz (px ,py ,pz ); + Vector3f pnxyz (pnx,pny,pnz); + Vector3f prgb (pr ,pg ,pb ); + float weight = 1.0/(dst_noise*dst_noise); + superpoint sp2 = superpoint(pxyz,pnxyz,prgb, weight, weight, frameid); + sp.merge(sp2); + isfused[dst_ind] = true; + } + } + } + } - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; + int nr_fused = 0; + int nr_mask = 0; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + nr_fused += isfused[ind]; + nr_mask += maskvec[ind] > 0; + } + } - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(!isfused[ind] && maskvec[ind]){ + float z = idepth*float(depthdata[ind]); + float nx = normalsdata[3*ind+0]; - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - if(dst_z > 0){ - float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 - residuals.push_back(diff_z); - weights.push_back(1.0); - } + if(z > 0 && nx != 2){ + float ny = normalsdata[3*ind+1]; + float nz = normalsdata[3*ind+2]; + + float x = (w - cx) * z * ifx; + float y = (h - cy) * z * ify; + + double noise = z * z; + + float px = m00*x + m01*y + m02*z + m03; + float py = m10*x + m11*y + m12*z + m13; + float pz = m20*x + m21*y + m22*z + m23; + float pnx = m00*nx + m01*ny + m02*nz; + float pny = m10*nx + m11*ny + m12*nz; + float pnz = m20*nx + m21*ny + m22*nz; + + float pb = rgbdata[3*ind+0]; + float pg = rgbdata[3*ind+1]; + float pr = rgbdata[3*ind+2]; + + Vector3f pxyz (px ,py ,pz ); + Vector3f pnxyz (pnx,pny,pnz); + Vector3f prgb (pr ,pg ,pb ); + float weight = 1.0/(noise*noise); + spvec.push_back(superpoint(pxyz,pnxyz,prgb, weight, weight, frameid)); } } } } -} -void ModelUpdater::recomputeScores(){ -// printf("recomputeScores\n"); - std::vector > ocs = getOcclusionScores(model->relativeposes,model->frames,model->modelmasks,false); - model->scores = getScores(ocs); + //delete[] isfused; - model->total_scores = 0; - for(unsigned int i = 0; i < model->scores.size(); i++){ - for(unsigned int j = 0; j < model->scores.size(); j++){ - model->total_scores += model->scores[i][j]; - } - } + if(debugg){ + pcl::PointCloud::Ptr cloud = getPCLnormalcloud(spvec); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } } -void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2){ - unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); - float * src_normalsdata = (float *)(frame1->normals.data); +vector ModelUpdater::getSuperPoints(vector cp, vector cf, vector cm, int type, bool debugg){ + vector spvec; + for(unsigned int i = 0; i < cp.size(); i++){addSuperPoints(spvec, cp[i], cf[i], cm[i], type,debugg);} + return spvec; +} - Camera * src_camera = frame1->camera; +double ModelUpdater::getCompareUtility(Matrix4d p, RGBDFrame* frame, ModelMask* mask, vector & cp, vector & cf, vector & cm){ + unsigned char * src_maskdata = (unsigned char *)(mask->mask.data); + unsigned char * src_rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame->depth.data); + float * src_normalsdata = (float *)(frame->normals.data); + + Camera * src_camera = frame->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + std::vector test_x; + std::vector test_y; + std::vector test_z; + std::vector test_nx; + std::vector test_ny; + std::vector test_nz; + + std::vector & testw = mask->testw; + std::vector & testh = mask->testh; + + for(unsigned int ind = 0; ind < testw.size();ind++){ + unsigned int src_w = testw[ind]; + unsigned int src_h = testh[ind]; + + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] == 255){ + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + test_x.push_back(src_x); + test_y.push_back(src_y); + test_z.push_back(src_z); + test_nx.push_back(src_nx); + test_ny.push_back(src_ny); + test_nz.push_back(src_nz); + } + } + } + + bool debugg = false; + unsigned int nrdata_start = test_x.size(); + + for(unsigned int c = 0; c < cp.size();c++){ + RGBDFrame* dst = cf[c]; + if(dst == frame){continue;} + ModelMask* dst_modelmask = cm[c]; + unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); + float * dst_normalsdata = (float *)(dst->normals.data); + + Matrix4d rp = cp[c].inverse()*p; + + float m00 = rp(0,0); float m01 = rp(0,1); float m02 = rp(0,2); float m03 = rp(0,3); + float m10 = rp(1,0); float m11 = rp(1,1); float m12 = rp(1,2); float m13 = rp(1,3); + float m20 = rp(2,0); float m21 = rp(2,1); float m22 = rp(2,2); float m23 = rp(2,3); + + Camera * dst_camera = dst->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + if(debugg){ + viewer->removeAllPointClouds(); + + pcl::PointCloud::Ptr cloud2 = dst->getPCLcloud(); + viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud2"); + } + + for(unsigned int ind = 0; ind < test_x.size();ind++){ + float src_nx = test_nx[ind]; + float src_ny = test_ny[ind]; + float src_nz = test_nz[ind]; + + float src_x = test_x[ind]; + float src_y = test_y[ind]; + float src_z = test_z[ind]; + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + pcl::PointXYZRGBNormal point; + if(debugg){ + point.x = tx; + point.y = ty; + point.z = tz; + point.r = 255; + point.g = 0; + point.b = 0; + } + + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + double dst_noise = dst_z * dst_z; + double point_noise = src_z * src_z; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; + + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE + //if(d < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + test_nx[ind] = test_nx.back(); test_nx.pop_back(); + test_ny[ind] = test_ny.back(); test_ny.pop_back(); + test_nz[ind] = test_nz.back(); test_nz.pop_back(); + test_x[ind] = test_x.back(); test_x.pop_back(); + test_y[ind] = test_y.back(); test_y.pop_back(); + test_z[ind] = test_z.back(); test_z.pop_back(); + ind--; + if(debugg){ + point.r = 0; + point.g = 255; + point.b = 0; + } + } + } + } + if(debugg){ + cloud->points.push_back(point); + } + } + + if(debugg){ + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } + } + return double(test_x.size())/double(nrdata_start); +} + +void ModelUpdater::getGoodCompareFrames(vector & cp, vector & cf, vector & cm){ + while(true){ + double worst = 1; + int worst_id = -1; + for(unsigned int i = 0; i < cp.size(); i++){ + double score = getCompareUtility(cp[i], cf[i], cm[i], cp, cf, cm); + //printf("%3.3i -> %f\n",i,score); + if(score < worst){ + worst = score; + worst_id = i; + } + } + + if(worst_id < 0){break;} + if(worst < 0.1){//worst frame can to 90% be represented by other frames + cp[worst_id] = cp.back(); + cp.pop_back(); + + cf[worst_id] = cf.back(); + cf.pop_back(); + + cm[worst_id] = cm.back(); + cm.pop_back(); + //printf("removing %i\n",worst_id); + }else{break;} + } +} + +void ModelUpdater::refine(double reg,bool useFullMask, int visualization){ + //return ; + printf("void ModelUpdater::refine()\n"); + + printf("%s::%i\n",__FILE__,__LINE__); + vector > ocs = computeOcclusionScore(model->submodels,model->submodels_relativeposes,1,false); + printf("%s::%i\n",__FILE__,__LINE__); + std::vector > scores = getScores(ocs); + printf("%s::%i\n",__FILE__,__LINE__); + + double sumscore_bef = 0; + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + sumscore_bef += scores[i][j]; + } + } + + MassFusionResults mfr; + MassRegistrationPPR * massreg = new MassRegistrationPPR(reg); + massreg->timeout = massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = 0; + massreg->maskstep = 4;//std::max(1,int(0.5+0.02*double(model->frames.size()))); + massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); + printf("maskstep: %i nomaskstep: %i\n",massreg->maskstep,massreg->nomaskstep); + massreg->nomask = true; + massreg->stopval = 0.001; + double step = model->submodels_relativeposes.size()*model->submodels_relativeposes.size(); + step *= 0.25; + step += 0.5; + massreg->maskstep = std::max(1.5,step); + massreg->addModelData(model); + mfr = massreg->getTransforms(model->submodels_relativeposes); + vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); + std::vector > scores2 = getScores(ocs2); + + double sumscore_aft = 0; + for(unsigned int i = 0; i < scores2.size(); i++){ + for(unsigned int j = 0; j < scores2.size(); j++){ + sumscore_aft += scores2[i][j]; + } + } + + printf("bef %f after %f\n",sumscore_bef,sumscore_aft); + if(sumscore_aft >= sumscore_bef){ + model->submodels_relativeposes = mfr.poses; + } +} + +void ModelUpdater::show(bool stop){ + //printf("void ModelUpdater::show()\n"); + pcl::PointCloud::Ptr cloud = model->getPCLcloud(1,false); + viewer->removeAllPointClouds(); + viewer->addPointCloud(cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + if(stop){ viewer->spin();} + else{ viewer->spinOnce();} + viewer->removeAllPointClouds(); +} + +//Backproject and prune occlusions +void ModelUpdater::pruneOcclusions(){} + +void ModelUpdater::computeResiduals(std::vector & residuals, std::vector & weights, + RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, + RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, + Eigen::Matrix4d p, bool debugg){ + + unsigned char * src_maskdata = (unsigned char *)(src_modelmask->mask.data); + unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(src->depth.data); + float * src_normalsdata = (float *)(src->normals.data); + + unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); + float * dst_normalsdata = (float *)(dst->normals.data); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + Camera * src_camera = src->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + Camera * dst_camera = dst->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + std::vector & testw = src_modelmask->testw; + std::vector & testh = src_modelmask->testh; + + unsigned int test_nrdata = testw.size(); + for(unsigned int ind = 0; ind < test_nrdata;ind++){ + unsigned int src_w = testw[ind]; + unsigned int src_h = testh[ind]; + + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ + float z = src_idepth*float(src_depthdata[src_ind]); + float nx = src_normalsdata[3*src_ind+0]; + + if(z > 0 && nx != 2){ + float ny = src_normalsdata[3*src_ind+1]; + float nz = src_normalsdata[3*src_ind+2]; + + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + //float tnx = m00*nx + m01*ny + m02*nz; + //float tny = m10*nx + m11*ny + m12*nz; + float tnz = m20*nx + m21*ny + m22*nz; + + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + if(dst_z > 0){ + float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 + residuals.push_back(diff_z); + weights.push_back(1.0); + } + } + } + } + } +} + +void ModelUpdater::recomputeScores(){ + // printf("recomputeScores\n"); + std::vector > ocs = getOcclusionScores(model->relativeposes,model->frames,model->modelmasks,false); + model->scores = getScores(ocs); + + model->total_scores = 0; + for(unsigned int i = 0; i < model->scores.size(); i++){ + for(unsigned int j = 0; j < model->scores.size(); j++){ + model->total_scores += model->scores[i][j]; + } + } +} + +void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2){ + unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); + float * src_normalsdata = (float *)(frame1->normals.data); + + Camera * src_camera = frame1->camera; const unsigned int src_width = src_camera->width; const unsigned int src_height = src_camera->height; const float src_idepth = src_camera->idepth_scale; @@ -1325,7 +1325,7 @@ void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weight float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - bool debugg = false; + bool debugg = false; pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); if(debugg){ @@ -1377,9 +1377,9 @@ void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weight point.x = dst_x; point.y = dst_y; point.z = dst_z; - point.r = 0; + point.r = 0; point.g = 0; - point.b = 255; + point.b = 255; dst_cloud->points[dst_ind] = point; } } @@ -1425,34 +1425,34 @@ void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weight float tny = m10*src_nx + m11*src_ny + m12*src_nz; float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); - //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); - //d *= src_z;//compare_mul; - //double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - - if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE - overlaps1[src_ind] += weights2[dst_ind]; - overlaps2[dst_ind] += weights1[src_ind]; - - total1[src_ind] += weights2[dst_ind]; - total2[dst_ind] += weights1[src_ind]; - if(debugg){ - dst_cloud->points[dst_ind].r = 0; - dst_cloud->points[dst_ind].g = 255; - dst_cloud->points[dst_ind].b = 0; - src_cloud->points[src_ind].r = 0; - src_cloud->points[src_ind].g = 255; - src_cloud->points[src_ind].b = 0; - } - }else if(tz < dst_z){ - total1[src_ind] += weights2[dst_ind]; - total2[dst_ind] += weights1[src_ind]; - if(debugg){ - src_cloud->points[src_ind].r = 255; - src_cloud->points[src_ind].g = 0; - src_cloud->points[src_ind].b = 255; - } - } + double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); + //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); + //d *= src_z;//compare_mul; + //double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + overlaps1[src_ind] += weights2[dst_ind]; + overlaps2[dst_ind] += weights1[src_ind]; + + total1[src_ind] += weights2[dst_ind]; + total2[dst_ind] += weights1[src_ind]; + if(debugg){ + dst_cloud->points[dst_ind].r = 0; + dst_cloud->points[dst_ind].g = 255; + dst_cloud->points[dst_ind].b = 0; + src_cloud->points[src_ind].r = 0; + src_cloud->points[src_ind].g = 255; + src_cloud->points[src_ind].b = 0; + } + }else if(tz < dst_z){ + total1[src_ind] += weights2[dst_ind]; + total2[dst_ind] += weights1[src_ind]; + if(debugg){ + src_cloud->points[src_ind].r = 255; + src_cloud->points[src_ind].g = 0; + src_cloud->points[src_ind].b = 255; + } + } } } } @@ -1461,19 +1461,19 @@ void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weight if(debugg){ viewer->removeAllPointClouds(); - viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); - viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); viewer->spin(); } } double interpolate( double val, double y0, double x0, double y1, double x1 ) {return (val-x0)*(y1-y0)/(x1-x0) + y0;} double base( double val ) { - if ( val <= -0.75 ) return 0; - else if ( val <= -0.25 ) return interpolate( val, 0.0, -0.75, 1.0, -0.25 ); - else if ( val <= 0.25 ) return 1.0; - else if ( val <= 0.75 ) return interpolate( val, 1.0, 0.25, 0.0, 0.75 ); - else return 0.0; + if ( val <= -0.75 ) return 0; + else if ( val <= -0.25 ) return interpolate( val, 0.0, -0.75, 1.0, -0.25 ); + else if ( val <= 0.25 ) return 1.0; + else if ( val <= 0.75 ) return interpolate( val, 1.0, 0.25, 0.0, 0.75 ); + else return 0.0; } double red( double gray ) { return base( gray - 0.5 ); } double green( double gray ) { return base( gray );} @@ -1482,27 +1482,27 @@ double blue( double gray ) { return base( gray + 0.5 );} cv::Mat getImageFromArray(unsigned int width, unsigned int height, double * arr){ double maxval = 0; for(unsigned int i = 0; i < width*height; i++){maxval = std::max(maxval,arr[i]);} - maxval = 1; + maxval = 1; cv::Mat m; m.create(height,width,CV_8UC3); unsigned char * data = m.data; - for(unsigned int i = 0; i < width*height; i++){ - double d = arr[i]/maxval; - if(d < 0){ - data[3*i+0] = 255.0;//blue(d); - data[3*i+1] = 0;//green(d); - data[3*i+2] = 255.0;//red(d); - }else{ - data[3*i+0] = 255.0*d;//blue(d); - data[3*i+1] = 255.0*d;//green(d); - data[3*i+2] = 255.0*d;//red(d); - } - } - - cv::namedWindow( "getImageFromArray", cv::WINDOW_AUTOSIZE ); - cv::imshow( "getImageFromArray",m); - cv::waitKey(0); + for(unsigned int i = 0; i < width*height; i++){ + double d = arr[i]/maxval; + if(d < 0){ + data[3*i+0] = 255.0;//blue(d); + data[3*i+1] = 0;//green(d); + data[3*i+2] = 255.0;//red(d); + }else{ + data[3*i+0] = 255.0*d;//blue(d); + data[3*i+1] = 255.0*d;//green(d); + data[3*i+2] = 255.0*d;//red(d); + } + } + + cv::namedWindow( "getImageFromArray", cv::WINDOW_AUTOSIZE ); + cv::imshow( "getImageFromArray",m); + cv::waitKey(0); return m; } @@ -1520,7 +1520,7 @@ void ModelUpdater::computeOcclusionAreas(vector cp, vector } } - weights_old.push_back(wo); + weights_old.push_back(wo); double ** overlaps = new double*[cf.size()]; double ** total = new double*[cf.size()]; @@ -1536,21 +1536,21 @@ void ModelUpdater::computeOcclusionAreas(vector cp, vector - for(unsigned int iter = 0; iter < 10; iter++){ - for(unsigned int i = 0; i < cf.size(); i++){ - unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; - for(unsigned int j = 0; j < nr_pixels; j++){ - overlaps[i][j] = 0; - total[i][j] = 0; - } - } -printf("iter %i\n",iter); + for(unsigned int iter = 0; iter < 10; iter++){ + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + for(unsigned int j = 0; j < nr_pixels; j++){ + overlaps[i][j] = 0; + total[i][j] = 0; + } + } + printf("iter %i\n",iter); for(unsigned int i = 0; i < cf.size(); i++){ for(unsigned int j = 0; j < cf.size(); j++){ - if(i == j){continue;} - //weightedocclusioncounts - Eigen::Matrix4d p = cp[i].inverse() * cp[j]; - getAreaWeights(p.inverse(), cf[i], wo[i], overlaps[i], total[i], cf[j], wo[j], overlaps[j], total[j]); + if(i == j){continue;} + //weightedocclusioncounts + Eigen::Matrix4d p = cp[i].inverse() * cp[j]; + getAreaWeights(p.inverse(), cf[i], wo[i], overlaps[i], total[i], cf[j], wo[j], overlaps[j], total[j]); } } @@ -1559,499 +1559,622 @@ printf("iter %i\n",iter); unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; wo[i] = new double[nr_pixels]; for(unsigned int j = 0; j < nr_pixels; j++){ - if(total[i][j] < 0.00001){ - wo[i][j] = 0.0; + if(total[i][j] < 0.00001){ + wo[i][j] = 0.0; }else{ - double fails = occlusion_penalty*(total[i][j]-overlaps[i][j]); - double others = overlaps[i][j]; - wo[i][j] = others/(fails+others); + double fails = occlusion_penalty*(total[i][j]-overlaps[i][j]); + double others = overlaps[i][j]; + wo[i][j] = others/(fails+others); } } } weights_old.push_back(wo); } - for(unsigned int i = 0; i < cf.size(); i++){ - unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; - double sum = 0; - for(unsigned int j = 0; j < nr_pixels; j++){ - sum += wo[i][j]; - } - sum /= double(nr_pixels); - for(unsigned int j = 0; j < nr_pixels; j++){ - if(total[i][j] == 0){ - wo[i][j] = sum; - } - } - } - for(unsigned int i = 0; i < cf.size(); i++){ - for(unsigned int j = 0; j < weights_old.size(); j++){ - getImageFromArray(cf[i]->camera->width, cf[i]->camera->height, weights_old[j][i]); - } - } + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + double sum = 0; + for(unsigned int j = 0; j < nr_pixels; j++){ + sum += wo[i][j]; + } + sum /= double(nr_pixels); + for(unsigned int j = 0; j < nr_pixels; j++){ + if(total[i][j] == 0){ + wo[i][j] = sum; + } + } + } + for(unsigned int i = 0; i < cf.size(); i++){ + for(unsigned int j = 0; j < weights_old.size(); j++){ + getImageFromArray(cf[i]->camera->width, cf[i]->camera->height, weights_old[j][i]); + } + } } void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ - unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); - float * src_normalsdata = (float *)(frame1->normals.data); - - Camera * src_camera = frame1->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - - unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); - float * dst_normalsdata = (float *)(frame2->normals.data); - unsigned char * dst_maskdata = (unsigned char *)(mask.data); + unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); + float * src_normalsdata = (float *)(frame1->normals.data); - Camera * dst_camera = frame2->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; + Camera * src_camera = frame1->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; + unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); + float * dst_normalsdata = (float *)(frame2->normals.data); + unsigned char * dst_maskdata = (unsigned char *)(mask.data); + + Camera * dst_camera = frame2->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); //bool debugg = true; - pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); - if(debugg){ - - src_cloud->points.resize(src_width*src_height); - for(unsigned int src_w = 0; src_w < src_width;src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; - - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; - point.r = 255; - point.g = 0; - point.b = 0; - src_cloud->points[src_ind] = point; - } - } - } - - dst_cloud->points.resize(dst_width*dst_height); - for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - int dst_ind = dst_h*dst_width+dst_w; - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - pcl::PointXYZRGBNormal point; - point.x = dst_x; - point.y = dst_y; - point.z = dst_z; - point.r = 0; - point.g = 0; - point.b = 255; - dst_cloud->points[dst_ind] = point; - } - } - } - } - - for(unsigned int src_w = 0; src_w < src_width;src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; - - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - double dst_noise = dst_z * dst_z; - double point_noise = src_z * src_z; - - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - - double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); - //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); - //d *= src_z;//compare_mul; - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - - if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); + if(debugg){ + + src_cloud->points.resize(src_width*src_height); + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + point.r = 255; + point.g = 0; + point.b = 0; + src_cloud->points[src_ind] = point; + } + } + } + + dst_cloud->points.resize(dst_width*dst_height); + for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + pcl::PointXYZRGBNormal point; + point.x = dst_x; + point.y = dst_y; + point.z = dst_z; + point.r = 0; + point.g = 0; + point.b = 255; + dst_cloud->points[dst_ind] = point; + } + } + } + } + + int informations = 0; + + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + double dst_noise = dst_z * dst_z; + double point_noise = src_z * src_z; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); + //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); + //d *= src_z;//compare_mul; + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE if(surface_angle > 0.8 && dst_maskdata[dst_ind] > 0){ - if(interframe_connectionId.size() > 0){ - double p_same = 0.9; - double not_p_same = 1-p_same; - double weight = -log(not_p_same); - - interframe_connectionId[src_ind+offset1].push_back(dst_ind+offset2); - interframe_connectionStrength[src_ind+offset1].push_back(weight); - } - overlaps[src_ind]++; - if(debugg){ - dst_cloud->points[dst_ind].r = 0; - dst_cloud->points[dst_ind].g = 255; - dst_cloud->points[dst_ind].b = 0; - src_cloud->points[src_ind].r = 0; - src_cloud->points[src_ind].g = 255; - src_cloud->points[src_ind].b = 0; - } - } - }else if(tz < dst_z){ - occlusions[src_ind]++; - if(debugg){ - src_cloud->points[src_ind].r = 255; - src_cloud->points[src_ind].g = 0; - src_cloud->points[src_ind].b = 255; - } - } - } - } - } - } - } - - if(debugg){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); - viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); - viewer->spin(); - } + if(interframe_connectionId.size() > 0){ + double p_same = 0.9; + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + + interframe_connectionId[src_ind+offset1].push_back(dst_ind+offset2); + interframe_connectionStrength[src_ind+offset1].push_back(weight); + } + overlaps[src_ind]++; + if(debugg){ + informations++; + dst_cloud->points[dst_ind].r = 0; + dst_cloud->points[dst_ind].g = 255; + dst_cloud->points[dst_ind].b = 0; + src_cloud->points[src_ind].r = 0; + src_cloud->points[src_ind].g = 255; + src_cloud->points[src_ind].b = 0; + } + } + }else if(tz < dst_z){ + occlusions[src_ind]++; + if(debugg){ + informations++; + src_cloud->points[src_ind].r = 255; + src_cloud->points[src_ind].g = 0; + src_cloud->points[src_ind].b = 255; + } + } + } + } + } + } + } + + if(debugg){printf("informations: %i\n",informations);} + if(debugg && informations > 0){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); + viewer->spin(); + } } std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ - cv::Mat src = frame->rgb.clone(); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); - const float idepth = frame->camera->idepth_scale; - - cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); - - unsigned char * srcdata = (unsigned char *)src.data; - unsigned int width = src.cols; - unsigned int height = src.rows; - unsigned int nr_pixels = width*height; - unsigned int nr_rgbedges = (width-2)*height + width*(height-2) + 2*(width-2)*(height-2); - - std::vector< std::vector > probs; - bool cross = false; - unsigned int chans = 3; - for(unsigned int c = 0; c < chans;c++){ - std::vector Xvec; - int dir; - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - dir = 1; - Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - - dir = width; - Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - } - } - - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); - for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} - - double stdval = 0; - for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} - stdval = sqrt(stdval/double(Xvec.size())); - - DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); - func->zeromean = true; - func->maxp = 0.999; - func->startreg = 0.0; - func->debugg_print = false; - func->bidir = true; - func->maxd = 256.0; - func->histogram_size = 256; - func->fixed_histogram_size = true; - func->startmaxd = func->maxd; - func->starthistogram_size = func->histogram_size; - func->blurval = 0.005; - func->stdval2 = stdval; - func->maxnoise = stdval; - func->reset(); - func->computeModel(X); - //printf("noise: %5.5f\n",func->getNoise()); - - std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} - std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} - std::vector dxy; dxy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dxy[i] = 0.5;} - std::vector dyx; dyx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dyx[i] = 0.5;} - for(unsigned int w = 1; w < width;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - - dir = 1; - dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - - dir = width; - dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - } - } - delete func; - - probs.push_back(dx); - probs.push_back(dy); - } - - { - std::vector Xvec; - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; - - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} - } - - if(h > 1){ - int dir = -width; - int other2 = ind+2*dir; - int other = ind+dir; - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; - - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} - } - } - } - - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); - for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} - - double stdval = 0; - for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} - stdval = sqrt(stdval/double(Xvec.size())); - - DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); - funcZ->zeromean = true; - funcZ->startreg = 0.002; - funcZ->debugg_print = false; - funcZ->bidir = true; - funcZ->maxp = 0.999999; - funcZ->maxd = 0.1; - funcZ->histogram_size = 100; - funcZ->fixed_histogram_size = true; - funcZ->startmaxd = funcZ->maxd; - funcZ->starthistogram_size = funcZ->histogram_size; - funcZ->blurval = 0.5; - funcZ->stdval2 = stdval; - funcZ->maxnoise = stdval; - funcZ->reset(); - funcZ->computeModel(X); - - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; - - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} - } - - if(h > 1){ - int dir = -width; - int other2 = ind+2*dir; - int other = ind+dir; - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; - - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} - } - } - } - - std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} - std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} - - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); + cv::Mat src = frame->rgb.clone(); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + const float idepth = frame->camera->idepth_scale; + + cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); + + unsigned char * srcdata = (unsigned char *)src.data; + unsigned int width = src.cols; + unsigned int height = src.rows; + unsigned int nr_pixels = width*height; + unsigned int nr_rgbedges = (width-2)*height + width*(height-2) + 2*(width-2)*(height-2); + + + cv::Mat src_dx; + src_dx.create(height,width,CV_32FC1); + float * src_dxdata = (float *)(src_dx.data); + + cv::Mat src_dy; + src_dy.create(height,width,CV_32FC1); + float * src_dydata = (float *)(src_dy.data); + + cv::Mat maxima_dx; + maxima_dx.create(height,width,CV_8UC1); + unsigned char * maxima_dxdata = (unsigned char *)(maxima_dx.data); + + cv::Mat maxima_dy; + maxima_dy.create(height,width,CV_8UC1); + unsigned char * maxima_dydata = (unsigned char *)(maxima_dy.data); + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + src_dxdata[ind] = 0; + src_dydata[ind] = 0; + } + } + + unsigned int chans = 3; + for(unsigned int c = 0; c < chans;c++){ + int dir; + + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 1; h < height;h++){ + int ind = h*width+w; + dir = 1; + src_dxdata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]) / 255.0)/3.0; + + dir = width; + src_dydata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]) / 255.0)/3.0; + } + } + + } + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + maxima_dxdata[ind] = 255*(src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] >= src_dxdata[ind+1]); + maxima_dydata[ind] = 255*(src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] >= src_dydata[ind+width]); + } + } + + + + + std::vector< std::vector > probs; + bool cross = false; + //unsigned int chans = 3; + for(unsigned int c = 0; c < chans;c++){ + std::vector Xvec; + int dir; + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + dir = 1; + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = width; + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + } + } + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + func->zeromean = true; + func->maxp = 0.99; + func->startreg = 0.0; + func->debugg_print = false; + func->bidir = true; + func->maxd = 256.0; + func->histogram_size = 256; + func->fixed_histogram_size = true; + func->startmaxd = func->maxd; + func->starthistogram_size = func->histogram_size; + func->blurval = 0.005; + func->stdval2 = stdval; + func->maxnoise = stdval; + func->reset(); + func->computeModel(X); + //printf("noise: %5.5f\n",func->getNoise()); + + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + std::vector dxy; dxy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dxy[i] = 0.5;} + std::vector dyx; dyx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dyx[i] = 0.5;} + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + + dir = 1; + dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = width; + dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + } + } + delete func; + + probs.push_back(dx); + probs.push_back(dy); + } + + { + std::vector Xvec; + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; - - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + } + } + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); + funcZ->zeromean = true; + funcZ->startreg = 0.002; + funcZ->debugg_print = false; + funcZ->bidir = true; + funcZ->maxp = 0.999999; + funcZ->maxd = 0.1; + funcZ->histogram_size = 100; + funcZ->fixed_histogram_size = true; + funcZ->startmaxd = funcZ->maxd; + funcZ->starthistogram_size = funcZ->histogram_size; + funcZ->blurval = 0.5; + funcZ->stdval2 = stdval; + funcZ->maxnoise = stdval; + funcZ->reset(); + funcZ->computeModel(X); + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + } + } + + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + + float dz = fabs(z-z2); + + //if(z3 > 0){z2 = 2*z2-z3;} + if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} + + //dz = fabs(z-z2); + //dz = std::min(dz,fabs(z-z2)); + + if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + + float dz = fabs(z-z2); - float dz = fabs(z-z2); + //if(z3 > 0){z2 = 2*z2-z3;} + //if(z3 > 0){dz = std::min(dz,fabs(z- 2*z2-z3));} - //if(z3 > 0){z2 = 2*z2-z3;} - if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} - //dz = fabs(z-z2); - //dz = std::min(dz,fabs(z-z2)); + //dz = std::min(dz,fabs(z-z2)); + if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} + if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} - if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} - } + // int other2 = ind+2*dir; + // int other = ind+dir; - if(h > 1){ - int dir = -width; + // float z3 = idepth*float(depthdata[other2]); + // float z2 = idepth*float(depthdata[other]); + // if(z3 > 0){z2 = 2*z2-z3;} - int other2 = ind+2*dir; - int other = ind+dir; + // float dz = z-z2; - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); + // if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + } + } + } + + delete funcZ; + probs.push_back(dx); + probs.push_back(dy); + } + //return probs; - float dz = fabs(z-z2); + std::vector dxc; + dxc.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0.5;} - //if(z3 > 0){z2 = 2*z2-z3;} - //if(z3 > 0){dz = std::min(dz,fabs(z- 2*z2-z3));} + std::vector dyc; + dyc.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){dyc[i] = 0.5;} + cv::Mat col_probs; + col_probs.create(height,width,CV_32FC3); + float * cpdata = (float *)col_probs.data; - //dz = std::min(dz,fabs(z-z2)); - if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} - if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; -// int other2 = ind+2*dir; -// int other = ind+dir; + cpdata[3*ind+0] = 0; + cpdata[3*ind+1] = 0; + cpdata[3*ind+2] = 0; -// float z3 = idepth*float(depthdata[other2]); -// float z2 = idepth*float(depthdata[other]); -// if(z3 > 0){z2 = 2*z2-z3;} + if(w > 0 && w < width-1){ + double ax = 0.5; + double bx = 0.5; + for(unsigned int p = 0; p < probs.size()-2; p+=2){ + double pr = probs[p][ind]; + ax *= pr; + bx *= 1.0-pr; + } + double px = ax/(ax+bx); + cpdata[3*ind+1] = (1-px) * float(maxima_dxdata[ind] > 0); + dxc[ind] = 1-cpdata[3*ind+1]; + } -// float dz = z-z2; + if(h > 0 && h < height-1){ -// if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} - } - } - } + double ay = 0.5; + double by = 0.5; + for(unsigned int p = 1; p < probs.size()-2; p+=2){ + double pr = probs[p][ind]; + ay *= pr; + by *= 1.0-pr; + } + double py = ay/(ay+by); + cpdata[3*ind+2] = (1-py) * float(maxima_dydata[ind] > 0); + dyc[ind] = 1-cpdata[3*ind+2]; + } + } + } - delete funcZ; - probs.push_back(dx); - probs.push_back(dy); - } - return probs; + std::vector< std::vector > probs2; + probs2.push_back(dxc); + probs2.push_back(dyc); + probs2.push_back(probs[probs.size()-2]); + probs2.push_back(probs.back()); + + // cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); + // cv::namedWindow( "col_probs", cv::WINDOW_AUTOSIZE ); cv::imshow( "col_probs", col_probs); + // cv::namedWindow( "src_dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_dx", cv::abs(src_dx)); + // cv::namedWindow( "src_dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_dy", cv::abs(src_dy)); + // cv::namedWindow( "maxima_dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "maxima_dx",maxima_dx); + // cv::namedWindow( "maxima_dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "maxima_dy",maxima_dy); + + // cv::waitKey(0); + return probs2; } std::vector doInference(std::vector & prior, std::vector< std::vector > & connectionId, std::vector< std::vector > & connectionStrength){ - double start_inf = getTime(); - - unsigned int nr_data = prior.size(); - unsigned int nr_edges = 0; - for(unsigned int j = 0; j < nr_data;j++){ - nr_edges += connectionId[j].size(); - } - - gc::Graph * g = new gc::Graph(nr_data,nr_edges); - for(unsigned int i = 0; i < nr_data;i++){ - g -> add_node(); - double p_fg = prior[i]; - if(p_fg < 0){ - g -> add_tweights( i, 0, 0 ); - continue; - } - double p_bg = 1-p_fg; - double weightFG = -log(p_fg); - double weightBG = -log(p_bg); - g -> add_tweights( i, weightFG, weightBG ); - } - - for(unsigned int i = 0; i < nr_data;i++){ + double start_inf = getTime(); + + unsigned int nr_data = prior.size(); + unsigned int nr_edges = 0; + for(unsigned int j = 0; j < nr_data;j++){ + nr_edges += connectionId[j].size(); + } + + gc::Graph * g = new gc::Graph(nr_data,nr_edges); + for(unsigned int i = 0; i < nr_data;i++){ + g -> add_node(); + double p_fg = prior[i]; + if(p_fg < 0){ + g -> add_tweights( i, 0, 0 ); + continue; + } + double p_bg = 1-p_fg; + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + g -> add_tweights( i, weightFG, weightBG ); + } + + for(unsigned int i = 0; i < nr_data;i++){ for(unsigned int j = 0; j < connectionId[i].size();j++){ - double weight = connectionStrength[i][j]; - g -> add_edge( i, connectionId[i][j], weight, weight ); - //try{g -> add_edge( i, connectionId[i][j], weight, weight );} - //catch(std::exception ex){printf("%i %i -> weight: %20.20f\n",i,connectionId[i],weight);exit(0);} - } - } + double weight = connectionStrength[i][j]; + g -> add_edge( i, connectionId[i][j], weight, weight ); + //try{g -> add_edge( i, connectionId[i][j], weight, weight );} + //catch(std::exception ex){printf("%i %i -> weight: %20.20f\n",i,connectionId[i],weight);exit(0);} + } + } - g -> maxflow(); + g -> maxflow(); - std::vector labels; - labels.resize(nr_data); - for(unsigned int i = 0; i < nr_data;i++){labels[i] = g->what_segment(i);} - delete g; + std::vector labels; + labels.resize(nr_data); + for(unsigned int i = 0; i < nr_data;i++){labels[i] = g->what_segment(i);} + delete g; - double end_inf = getTime(); - printf("nr data: %i nr edges: %i inference time: %10.10fs\n",nr_data,nr_edges,end_inf-start_inf); + double end_inf = getTime(); + printf("nr data: %i nr edges: %i inference time: %10.10fs\n",nr_data,nr_edges,end_inf-start_inf); - return labels; + return labels; } void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, double debugg){ @@ -2202,15 +2325,15 @@ void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * ove if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE if(surface_angle > 0.9){ overlaps[src_ind] = std::max(overlaps[src_ind],0.9); -// overlaps[src_ind]++; -// if(debugg){ -//// dst_cloud->points[dst_ind].r = 0; -//// dst_cloud->points[dst_ind].g = 255; -//// dst_cloud->points[dst_ind].b = 0; -// src_cloud->points[src_ind].r = 0; -// src_cloud->points[src_ind].g = 255; -// src_cloud->points[src_ind].b = 0; -// } + // overlaps[src_ind]++; + // if(debugg){ + //// dst_cloud->points[dst_ind].r = 0; + //// dst_cloud->points[dst_ind].g = 255; + //// dst_cloud->points[dst_ind].b = 0; + // src_cloud->points[src_ind].r = 0; + // src_cloud->points[src_ind].g = 255; + // src_cloud->points[src_ind].b = 0; + // } } }else if(tz < dst_z){ occlusions[src_ind] = std::max(occlusions[src_ind],0.9); @@ -2235,15 +2358,15 @@ void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * ove } void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ - int tot_nr_pixels = 0; - std::vector offsets; + int tot_nr_pixels = 0; + std::vector offsets; - for(unsigned int i = 0; i < frames.size(); i++){ - offsets.push_back(tot_nr_pixels); - unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; - tot_nr_pixels += nr_pixels; - } - printf("tot_nr_pixels: %i\n",tot_nr_pixels); + for(unsigned int i = 0; i < frames.size(); i++){ + offsets.push_back(tot_nr_pixels); + unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; + tot_nr_pixels += nr_pixels; + } + printf("tot_nr_pixels: %i\n",tot_nr_pixels); int current_point = 0; @@ -2382,30 +2505,30 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vectorremoveAllPointClouds(); -// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); -// viewer->spin(); + // viewer->removeAllPointClouds(); + // viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + // viewer->spin(); -// for(unsigned int w = 0; w < width;w++){ -// for(unsigned int h = 0; h < height;h++){ -// int ind = h*width+w; -// float z = idepth*float(depthdata[ind]); -// if(z > 0){ -// float x = (float(w) - cx) * z * ifx; -// float y = (float(h) - cy) * z * ify; + // for(unsigned int w = 0; w < width;w++){ + // for(unsigned int h = 0; h < height;h++){ + // int ind = h*width+w; + // float z = idepth*float(depthdata[ind]); + // if(z > 0){ + // float x = (float(w) - cx) * z * ifx; + // float y = (float(h) - cy) * z * ify; -// pcl::PointXYZRGBNormal point; -// point.x = m00*x + m01*y + m02*z + m03; -// point.y = m10*x + m11*y + m12*z + m13; -// point.z = m20*x + m21*y + m22*z + m23; -// point.b = rgbdata[3*ind+0]; -// point.g = rgbdata[3*ind+1]; -// point.r = rgbdata[3*ind+2]; -// cloud->points.push_back(point); -// } -// } -// } + // pcl::PointXYZRGBNormal point; + // point.x = m00*x + m01*y + m02*z + m03; + // point.y = m10*x + m11*y + m12*z + m13; + // point.z = m20*x + m21*y + m22*z + m23; + // point.b = rgbdata[3*ind+0]; + // point.g = rgbdata[3*ind+1]; + // point.r = rgbdata[3*ind+2]; + // cloud->points.push_back(point); + // } + // } + // } delete[] current_occlusions; @@ -2558,55 +2681,58 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg){ - std::vector newmasks; + std::vector newmasks; + + double maxprob = 0.7; - double maxprob = 0.7; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud1 (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud1 (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); + pcl::PointCloud::Ptr bgcloud (new pcl::PointCloud); - int tot_nr_pixels = 0; + int tot_nr_pixels = 0; std::vector offsets; - for(unsigned int i = 0; i < frames.size(); i++){ + for(unsigned int i = 0; i < frames.size(); i++){ offsets.push_back(tot_nr_pixels); unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; - tot_nr_pixels += nr_pixels; - } + tot_nr_pixels += nr_pixels; + } - //Graph... - std::vector prior; - std::vector< std::vector > connectionId; - std::vector< std::vector > connectionStrength; - std::vector< std::vector > interframe_connectionId; - std::vector< std::vector > interframe_connectionStrength; + //Graph... + std::vector prior; + std::vector< std::vector > connectionId; + std::vector< std::vector > connectionStrength; + std::vector< std::vector > interframe_connectionId; + std::vector< std::vector > interframe_connectionStrength; - prior.resize(tot_nr_pixels); - connectionId.resize(tot_nr_pixels); - connectionStrength.resize(tot_nr_pixels); - interframe_connectionId.resize(tot_nr_pixels); - interframe_connectionStrength.resize(tot_nr_pixels); + prior.resize(tot_nr_pixels); + connectionId.resize(tot_nr_pixels); + connectionStrength.resize(tot_nr_pixels); + interframe_connectionId.resize(tot_nr_pixels); + interframe_connectionStrength.resize(tot_nr_pixels); for(unsigned int i = 0; i < frames.size(); i++){ + printf("frame: %i\n",i); int offset = offsets[i]; - RGBDFrame * frame = frames[i]; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - - Camera * camera = frame->camera; + RGBDFrame * frame = frames[i]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + + Camera * camera = frame->camera; const unsigned int width = camera->width; const unsigned int height = camera->height; - unsigned int nr_pixels = frame->camera->width * frame->camera->height; + unsigned int nr_pixels = frame->camera->width * frame->camera->height; double * overlaps = new double[nr_pixels]; double * occlusions = new double[nr_pixels]; for(unsigned int j = 0; j < nr_pixels; j++){ @@ -2614,46 +2740,46 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector > interframe_connectionId_tmp; - std::vector< std::vector > interframe_connectionStrength_tmp; + std::vector< std::vector > interframe_connectionId_tmp; + std::vector< std::vector > interframe_connectionStrength_tmp; for(unsigned int j = 0; j < bgcp.size(); j++){ - if(frame == bgcf[j]){continue;} + if(frame == bgcf[j]){continue;} Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,debugg); } for(unsigned int j = 0; j < nr_pixels; j++){ occlusions[j] = 0; } - + printf("CHECK AGAINST BACKGROUND\n"); for(unsigned int j = 0; j < cp.size(); j++){ - if(frame == cf[j]){continue;} + if(frame == cf[j]){continue;} Eigen::Matrix4d p = poses[i].inverse() * cp[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, cf[j],mm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, cf[j],mm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,debugg); } - std::vector< std::vector > probs = getImageProbs(frames[i],5); + std::vector< std::vector > probs = getImageProbs(frames[i],5); - std::vector frame_prior; - std::vector< std::vector > frame_connectionId; - std::vector< std::vector > frame_connectionStrength; + std::vector frame_prior; + std::vector< std::vector > frame_connectionId; + std::vector< std::vector > frame_connectionStrength; - frame_prior.resize(nr_pixels); - frame_connectionId.resize(nr_pixels); - frame_connectionStrength.resize(nr_pixels); + frame_prior.resize(nr_pixels); + frame_connectionId.resize(nr_pixels); + frame_connectionStrength.resize(nr_pixels); printf("starting partition\n"); double start_part = getTime(); - unsigned int nr_data = width*height; - for(unsigned int j = 0; j < nr_data;j++){ + unsigned int nr_data = width*height; + for(unsigned int j = 0; j < nr_data;j++){ if(depthdata[j] == 0){ frame_prior[j] = -1; prior[offset+j] = -1; continue; } - double p_fg = 0.49999; + double p_fg = 0.499999; if(occlusions[j] >= 1){ p_fg = maxprob;} else if(overlaps[j] >= 1){ p_fg = 0.4;} @@ -2664,57 +2790,64 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector 0 && w < width-1){ - double ax = 0.5; - double bx = 0.5; - for(unsigned int p = 0; p < probs.size(); p+=2){ - double pr = probs[p][ind]; - ax *= pr; - bx *= 1.0-pr; - } - double px = ax/(ax+bx); - - int other = ind-1; - double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); + if(w > 0 && w < width-1){ + double ax = 0.5; + double bx = 0.5; + for(unsigned int p = 0; p < probs.size(); p+=2){ + double pr = probs[p][ind]; + ax *= pr; + bx *= 1.0-pr; + } + double px = ax/(ax+bx); + + int other = ind-1; + //double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); + //double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); + //double p_same = std::min(probs[probs.size()-2][ind],maxprob_same); + double p_same = std::max(1-maxprob_same,std::min(px,maxprob_same)); double not_p_same = 1-p_same; double weight = -log(not_p_same); - if(!std::isnan(weight) && weight > 0){ - frame_connectionId[ind].push_back(other); - frame_connectionStrength[ind].push_back(weight); + if(!std::isnan(weight) && weight > 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); - connectionId[ind+offset].push_back(other+offset); - connectionStrength[ind+offset].push_back(weight); - } + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } } - if(h > 0 && h < height-1){ + if(h > 0 && h < height-1){ - double ay = 0.5; - double by = 0.5; - for(unsigned int p = 1; p < probs.size(); p+=2){ - double pr = probs[p][ind]; - ay *= pr; - by *= 1.0-pr; - } - double py = ay/(ay+by); + double ay = 0.5; + double by = 0.5; + for(unsigned int p = 1; p < probs.size(); p+=2){ + double pr = probs[p][ind]; + ay *= pr; + by *= 1.0-pr; + } + double py = ay/(ay+by); - int other = ind-width; - double p_same = std::max(probs[probs.size()-1][ind],std::min(py,maxprob_same)); + int other = ind-width; + //double p_same = std::max(probs[probs.size()-1][ind],std::min(py,maxprob_same)); + + //double p_same = std::min(py,maxprob_same); + double p_same = std::max(1-maxprob_same,std::min(py,maxprob_same)); + //double p_same = std::min(probs[probs.size()-1][ind],maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); - if(!std::isnan(weight) && weight > 0){ - frame_connectionId[ind].push_back(other); - frame_connectionStrength[ind].push_back(weight); + if(!std::isnan(weight) && weight > 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); - connectionId[ind+offset].push_back(other+offset); - connectionStrength[ind+offset].push_back(weight); - } + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } } } } @@ -2724,292 +2857,238 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); - - Eigen::Matrix4d p = poses[i]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - pcl::PointXYZRGBNormal point; - point.x = m00*x + m01*y + m02*z + m03; - point.y = m10*x + m11*y + m12*z + m13; - point.z = m20*x + m21*y + m22*z + m23; - point.b = rgbdata[3*ind+0]; - point.g = rgbdata[3*ind+1]; - point.r = rgbdata[3*ind+2]; - - cloud->points.push_back(point); - } - } - } - - bool debugg2 = debugg; - if(debugg2){ - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; - - point.r = 0; - point.g = 255; - point.b = 255; - - if(overlaps[ind] >= 1){ - point.r = 0; - point.g = 255; - point.b = 0; - } - if(occlusions[ind] >= 1){ - point.r = 255; - point.g = 0; - point.b = 0; - } - //cloud->points[ind] = point; - cloud2->points.push_back(point); - } - } - } - } - - for(unsigned int j = 0; j < nr_pixels; j++){ - occlusions[j] = 0; - } - - for(unsigned int j = 0; j < frames.size(); j++){ - if(frame == frames[j]){continue;} - Eigen::Matrix4d p = poses[i].inverse() * poses[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, frames[j],masks[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); - } + //std::vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); + + Eigen::Matrix4d p = poses[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + pcl::PointXYZRGBNormal point; + point.x = m00*x + m01*y + m02*z + m03; + point.y = m10*x + m11*y + m12*z + m13; + point.z = m20*x + m21*y + m22*z + m23; + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; + + cloud->points.push_back(point); + } + } + } + + bool debugg2 = debugg; + if(debugg2){ + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + + point.r = 0; + point.g = 255; + point.b = 255; + + if(overlaps[ind] >= 1){ + point.r = 0; + point.g = 255; + point.b = 0; + } + if(occlusions[ind] >= 1){ + point.r = 255; + point.g = 0; + point.b = 0; + } + //cloud->points[ind] = point; + cloud2->points.push_back(point); + } + } + } + } + + for(unsigned int j = 0; j < nr_pixels; j++){ + occlusions[j] = 0; + } + + for(unsigned int j = 0; j < frames.size(); j++){ + if(frame == frames[j]){continue;} + Eigen::Matrix4d p = poses[i].inverse() * poses[j]; + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, frames[j],masks[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); + } //Time to compute external masks... delete[] overlaps; delete[] occlusions; } - //std::vector global_labels = doInference(prior,connectionId,connectionStrength); - - for(unsigned int i = 0; i < interframe_connectionId.size(); i++){ - for(unsigned int j = 0; j < interframe_connectionId[i].size(); j++){ - connectionId[i].push_back(interframe_connectionId[i][j]); - connectionStrength[i].push_back(interframe_connectionStrength[i][j]); - } - } - - std::vector improvedglobal_labels = doInference(prior,connectionId,connectionStrength); - - for(unsigned int f = 0; f < frames.size(); f++){ - int offset = offsets[f]; - RGBDFrame * frame = frames[f]; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); -// unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - unsigned int nr_pixels = frame->camera->width * frame->camera->height; - -// cv::Mat originalmask; -// originalmask.create(height,width,CV_8UC1); -// unsigned char * originaldata = (unsigned char *)(originalmask.data); - - cv::Mat improvedmask; - improvedmask.create(height,width,CV_8UC1); - unsigned char * improveddata = (unsigned char *)(improvedmask.data); - -// cv::Mat diffmask; -// diffmask.create(height,width,CV_8UC3); -// unsigned char * diffdata = (unsigned char *)(diffmask.data); - - for(unsigned int i = 0; i < nr_pixels;i++){ - //internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); - //originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph::SOURCE)); - improveddata[i] = 255.0*((depthdata[i] == 0) || (improvedglobal_labels[i+offset] == gc::Graph::SOURCE)); -// if(improveddata[i] < originaldata[i]){ -// diffdata[3*i+0] = 0; -// diffdata[3*i+1] = 255; -// diffdata[3*i+2] = 0; -// }else if(improveddata[i] > originaldata[i]){ -// diffdata[3*i+0] = 0; -// diffdata[3*i+1] = 0; -// diffdata[3*i+2] = 255; -// }else{ -// diffdata[3*i+0] = 0; -// diffdata[3*i+1] = 0; -// diffdata[3*i+2] = 0; -// } - - - } - -// frame->show(false); -// cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "Original", originalmask ); -// cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "Improved", improvedmask ); -// cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "Difference", diffmask ); -// cv::waitKey(0); - newmasks.push_back(improvedmask); - } - - - if(debugg){ - -/* - for(unsigned int i = 0; i < frames.size(); i++){ - int offset = offsets[i]; - RGBDFrame * frame = frames[i]; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - Eigen::Matrix4d p = poses[i]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - - unsigned int nr_pixels = frame->camera->width * frame->camera->height; - - cv::Mat originalmask; - originalmask.create(height,width,CV_8UC1); - unsigned char * originaldata = (unsigned char *)(originalmask.data); - - cv::Mat improvedmask; - improvedmask.create(height,width,CV_8UC1); - unsigned char * improveddata = (unsigned char *)(improvedmask.data); - - cv::Mat diffmask; - diffmask.create(height,width,CV_8UC3); - unsigned char * diffdata = (unsigned char *)(diffmask.data); - - for(unsigned int i = 0; i < nr_pixels;i++){ - //internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); - originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph::SOURCE)); - improveddata[i] = 255.0*((depthdata[i] == 0) || (improvedglobal_labels[i+offset] == gc::Graph::SOURCE)); - if(improveddata[i] < originaldata[i]){ - diffdata[3*i+0] = 0; - diffdata[3*i+1] = 255; - diffdata[3*i+2] = 0; - }else if(improveddata[i] > originaldata[i]){ - diffdata[3*i+0] = 0; - diffdata[3*i+1] = 0; - diffdata[3*i+2] = 255; - }else{ - diffdata[3*i+0] = 0; - diffdata[3*i+1] = 0; - diffdata[3*i+2] = 0; - } - } - -// frame->show(false); -// cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "Original", originalmask ); -// cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "Improved", improvedmask ); -// cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "Difference", diffmask ); -// cv::waitKey(0); - - - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; - - //(improvedglobal_labels[i+offset] == gc::Graph::SOURCE) - - //if (labels[ind] == gc::Graph::SOURCE){ - //if (improvedglobal_labels[ind+offset] == gc::Graph::SOURCE){ - if(improveddata[ind] == 0){ - point.r = 255; - point.g = 0; - point.b = 0; - }else{ - point.b = rgbdata[3*ind+0]; - point.g = rgbdata[3*ind+1]; - point.r = rgbdata[3*ind+2]; - } - cloud1->points.push_back(point); - } - } - } - - - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud1, pcl::visualization::PointCloudColorHandlerRGBField(cloud1), "scloud"); - viewer->spin(); - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - viewer->spin(); - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "scloud"); - viewer->spin(); - - viewer->removeAllPointClouds(); - */ - } + //std::vector global_labels = doInference(prior,connectionId,connectionStrength); + + for(unsigned int i = 0; i < interframe_connectionId.size(); i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size(); j++){ + connectionId[i].push_back(interframe_connectionId[i][j]); + connectionStrength[i].push_back(interframe_connectionStrength[i][j]); + } + } + + std::vector improvedglobal_labels = doInference(prior,connectionId,connectionStrength); + + for(unsigned int f = 0; f < frames.size(); f++){ + int offset = offsets[f]; + RGBDFrame * frame = frames[f]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + // unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + unsigned int nr_pixels = frame->camera->width * frame->camera->height; + + // cv::Mat originalmask; + // originalmask.create(height,width,CV_8UC1); + // unsigned char * originaldata = (unsigned char *)(originalmask.data); + + cv::Mat improvedmask; + improvedmask.create(height,width,CV_8UC1); + unsigned char * improveddata = (unsigned char *)(improvedmask.data); + + // cv::Mat diffmask; + // diffmask.create(height,width,CV_8UC3); + // unsigned char * diffdata = (unsigned char *)(diffmask.data); + + for(unsigned int i = 0; i < nr_pixels;i++){ + //internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); + //originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph::SOURCE)); + improveddata[i] = 255.0*((depthdata[i] == 0) || (improvedglobal_labels[i+offset] == gc::Graph::SOURCE)); + // if(improveddata[i] < originaldata[i]){ + // diffdata[3*i+0] = 0; + // diffdata[3*i+1] = 255; + // diffdata[3*i+2] = 0; + // }else if(improveddata[i] > originaldata[i]){ + // diffdata[3*i+0] = 0; + // diffdata[3*i+1] = 0; + // diffdata[3*i+2] = 255; + // }else{ + // diffdata[3*i+0] = 0; + // diffdata[3*i+1] = 0; + // diffdata[3*i+2] = 0; + // } + + + } + + // frame->show(false); + // cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); + // cv::imshow( "Original", originalmask ); + // cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); + // cv::imshow( "Improved", improvedmask ); + // cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); + // cv::imshow( "Difference", diffmask ); + // cv::waitKey(0); + newmasks.push_back(improvedmask); + } + + + if(debugg){ + + + for(unsigned int i = 0; i < bgcf.size(); i++){ + RGBDFrame * frame = bgcf[i]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + Eigen::Matrix4d p = bgcp[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; + + bgcloud->points.push_back(point); + } + } + } + } + /* + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud1, pcl::visualization::PointCloudColorHandlerRGBField(cloud1), "scloud"); + viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + viewer->spin(); + */ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "scloud"); + viewer->addPointCloud (bgcloud, pcl::visualization::PointCloudColorHandlerRGBField(bgcloud), "bgcloud"); + viewer->spin(); + + viewer->removeAllPointClouds(); + + } return newmasks; } std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector oldmasks){ std::vector newmasks; - for(unsigned int i = 0; i < cf.size(); i++){ + for(unsigned int i = 0; i < cf.size(); i++){ unsigned short * depthdata = (unsigned short *)(cf[i]->depth.data); Camera * camera = cf[i]->camera; @@ -3017,104 +3096,104 @@ std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eig const unsigned int height = camera->height; - unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; - double * overlaps = new double[nr_pixels]; - double * occlusions = new double[nr_pixels]; - for(unsigned int j = 0; j < nr_pixels; j++){ - overlaps[j] = 0; - occlusions[j] = 0; - } + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + double * overlaps = new double[nr_pixels]; + double * occlusions = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + overlaps[j] = 0; + occlusions[j] = 0; + } - std::vector< std::vector > interframe_connectionId_tmp; - std::vector< std::vector > interframe_connectionStrength_tmp; + std::vector< std::vector > interframe_connectionId_tmp; + std::vector< std::vector > interframe_connectionStrength_tmp; -// for(unsigned int j = 0; j < bgcp.size(); j++){ -// if(frame == bgcf[j]){continue;} -// Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; -// getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); -// } + // for(unsigned int j = 0; j < bgcp.size(); j++){ + // if(frame == bgcf[j]){continue;} + // Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; + // getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); + // } - for(unsigned int j = 0; j < cf.size(); j++){ + for(unsigned int j = 0; j < cf.size(); j++){ if(i == j){continue;} - Eigen::Matrix4d p = cp[i].inverse() * cp[j]; - getDynamicWeights(false,p.inverse(), cf[i], overlaps, occlusions, cf[j],oldmasks[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp); - } - std::vector< std::vector > probs = getImageProbs(cf[i],5); - - printf("starting partition\n"); - double start_part = getTime(); - unsigned int nr_data = width*height; + Eigen::Matrix4d p = cp[i].inverse() * cp[j]; + getDynamicWeights(false,p.inverse(), cf[i], overlaps, occlusions, cf[j],oldmasks[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp); + } + std::vector< std::vector > probs = getImageProbs(cf[i],5); + + printf("starting partition\n"); + double start_part = getTime(); + unsigned int nr_data = width*height; gc::Graph *g = new gc::Graph( nr_data, width*(height-1) + (width-1)*height); - double maxprob = 0.7; - for(unsigned int j = 0; j < nr_data;j++){ - g -> add_node(); + double maxprob = 0.7; + for(unsigned int j = 0; j < nr_data;j++){ + g -> add_node(); if(depthdata[j] == 0){ - g -> add_tweights(j,0,0); - continue; - } + g -> add_tweights(j,0,0); + continue; + } - double p_fg = 0.499; + double p_fg = 0.499; - if(occlusions[j] >= 1){ p_fg = maxprob;} - else if(overlaps[j] >= 1){ p_fg = 0.4;} + if(occlusions[j] >= 1){ p_fg = maxprob;} + else if(overlaps[j] >= 1){ p_fg = 0.4;} - p_fg = std::max(1-maxprob,std::min(maxprob,p_fg)); + p_fg = std::max(1-maxprob,std::min(maxprob,p_fg)); - double p_bg = 1-p_fg; - double weightFG = -log(p_fg); - double weightBG = -log(p_bg); - g -> add_tweights( j, weightFG, weightBG ); - } + double p_bg = 1-p_fg; + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + g -> add_tweights( j, weightFG, weightBG ); + } - double maxprob_same = 0.999999999; + double maxprob_same = 0.999999999; for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; - double ax = 0.5; - double bx = 0.5; - for(unsigned int p = 0; p < probs.size(); p+=2){ + double ax = 0.5; + double bx = 0.5; + for(unsigned int p = 0; p < probs.size(); p+=2){ double pr = probs[p][ind]; - ax *= pr; - bx *= 1.0-pr; - } - double px = ax/(ax+bx); - - double ay = 0.5; - double by = 0.5; - for(unsigned int p = 1; p < probs.size(); p+=2){ + ax *= pr; + bx *= 1.0-pr; + } + double px = ax/(ax+bx); + + double ay = 0.5; + double by = 0.5; + for(unsigned int p = 1; p < probs.size(); p+=2){ double pr = probs[p][ind]; - ay *= pr; - by *= 1.0-pr; - } - double py = ay/(ay+by); + ay *= pr; + by *= 1.0-pr; + } + double py = ay/(ay+by); if(w > 0){ int other = ind-1; double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); - double not_p_same = 1-p_same; + double not_p_same = 1-p_same; double weight = -log(not_p_same); g -> add_edge( ind, other, weight, weight ); - } + } if(h > 0){ int other = ind-width; double p_same = std::max(probs[probs.size()-1][ind],std::min(py,maxprob_same)); - double not_p_same = 1-p_same; - double weight = -log(not_p_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); g -> add_edge( ind, other, weight, weight ); - } - } - } + } + } + } - int flow = g -> maxflow(); - printf("flow: %i\n",flow); + int flow = g -> maxflow(); + printf("flow: %i\n",flow); - double end_part = getTime(); - printf("part time: %10.10fs\n",end_part-start_part); + double end_part = getTime(); + printf("part time: %10.10fs\n",end_part-start_part); cv::Mat internalmask; @@ -3128,121 +3207,121 @@ std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eig newmasks.push_back(internalmask); //Time to compute external masks... - delete[] overlaps; - delete[] occlusions; - } + delete[] overlaps; + delete[] occlusions; + } return newmasks; } OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * src_modelmask, RGBDFrame * dst, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step, bool debugg){ - OcclusionScore oc; - - unsigned char * src_maskdata = (unsigned char *)(src_modelmask->mask.data); - unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(src->depth.data); - float * src_normalsdata = (float *)(src->normals.data); - - unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); - unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); - float * dst_normalsdata = (float *)(dst->normals.data); - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - Camera * src_camera = src->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - - Camera * dst_camera = dst->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - - //bool debugg = false; - cv::Mat debugg_img; - unsigned char * debugg_img_data; - if(debugg){ - debugg_img = src->rgb.clone(); - debugg_img_data = (unsigned char *)(debugg_img.data); - } - - std::vector residuals; - std::vector angleweights; - std::vector weights; - std::vector ws; - std::vector hs; - - std::vector dst_ws; - std::vector dst_hs; - - residuals.reserve(src_width*src_height); - angleweights.reserve(src_width*src_height); - weights.reserve(src_width*src_height); - if(debugg){ - ws.reserve(src_width*src_height); - hs.reserve(src_width*src_height); - dst_ws.reserve(dst_width*dst_height); - dst_hs.reserve(dst_width*dst_height); - } - - double sum = 0; - double count = 0; - - std::vector & testw = src_modelmask->testw; - std::vector & testh = src_modelmask->testh; - - unsigned int test_nrdata = testw.size(); - unsigned int indstep = step; -// int indstep = std::max(1.0,double(test_nrdata)/double(step)); -// printf("indstep: %i\n",indstep); -// for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){ -// //for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ -// // for(unsigned int src_h = 0; src_h < src_height;src_h++){ -// unsigned int src_w = testw[ind]; -// unsigned int src_h = testh[ind]; - -// int src_ind0 = src_h*src_width+src_w-1; -// int src_ind1 = src_h*src_width+src_w; -// int src_ind2 = src_h*src_width+src_w+1; -// if(src_maskdata[src_ind0] == 255 && src_maskdata[src_ind1] == 255 && src_maskdata[src_ind2] == 255){// && p.z > 0 && !isnan(p.normal_x)){ -// float z0 = src_idepth*float(src_depthdata[src_ind0]); -// float z1 = src_idepth*float(src_depthdata[src_ind1]); -// float z2 = src_idepth*float(src_depthdata[src_ind2]); - -// double diff0 = (z0-z1)/(z0*z0+z1*z1); -// double diff1 = (z2-z1)/(z2*z2+z1*z1); -// if( fabs(diff0) < fabs(diff1)){ -// if(diff0 != 0){ -// sum += diff0*diff0; -// count++; -// } -// }else{ -// if(diff1 != 0){ -// sum += diff1*diff1; -// count++; -// } -// } -// } -// } -// double me = sqrt(sum/(count+1)); -// double pred = 1.3*sqrt(2)*me; -/* + OcclusionScore oc; + + unsigned char * src_maskdata = (unsigned char *)(src_modelmask->mask.data); + unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(src->depth.data); + float * src_normalsdata = (float *)(src->normals.data); + + unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); + float * dst_normalsdata = (float *)(dst->normals.data); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + Camera * src_camera = src->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + Camera * dst_camera = dst->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + //bool debugg = false; + cv::Mat debugg_img; + unsigned char * debugg_img_data; + if(debugg){ + debugg_img = src->rgb.clone(); + debugg_img_data = (unsigned char *)(debugg_img.data); + } + + std::vector residuals; + std::vector angleweights; + std::vector weights; + std::vector ws; + std::vector hs; + + std::vector dst_ws; + std::vector dst_hs; + + residuals.reserve(src_width*src_height); + angleweights.reserve(src_width*src_height); + weights.reserve(src_width*src_height); + if(debugg){ + ws.reserve(src_width*src_height); + hs.reserve(src_width*src_height); + dst_ws.reserve(dst_width*dst_height); + dst_hs.reserve(dst_width*dst_height); + } + + double sum = 0; + double count = 0; + + std::vector & testw = src_modelmask->testw; + std::vector & testh = src_modelmask->testh; + + unsigned int test_nrdata = testw.size(); + unsigned int indstep = step; + // int indstep = std::max(1.0,double(test_nrdata)/double(step)); + // printf("indstep: %i\n",indstep); + // for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){ + // //for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ + // // for(unsigned int src_h = 0; src_h < src_height;src_h++){ + // unsigned int src_w = testw[ind]; + // unsigned int src_h = testh[ind]; + + // int src_ind0 = src_h*src_width+src_w-1; + // int src_ind1 = src_h*src_width+src_w; + // int src_ind2 = src_h*src_width+src_w+1; + // if(src_maskdata[src_ind0] == 255 && src_maskdata[src_ind1] == 255 && src_maskdata[src_ind2] == 255){// && p.z > 0 && !isnan(p.normal_x)){ + // float z0 = src_idepth*float(src_depthdata[src_ind0]); + // float z1 = src_idepth*float(src_depthdata[src_ind1]); + // float z2 = src_idepth*float(src_depthdata[src_ind2]); + + // double diff0 = (z0-z1)/(z0*z0+z1*z1); + // double diff1 = (z2-z1)/(z2*z2+z1*z1); + // if( fabs(diff0) < fabs(diff1)){ + // if(diff0 != 0){ + // sum += diff0*diff0; + // count++; + // } + // }else{ + // if(diff1 != 0){ + // sum += diff1*diff1; + // count++; + // } + // } + // } + // } + // double me = sqrt(sum/(count+1)); + // double pred = 1.3*sqrt(2)*me; + /* for(unsigned int ind = 0; ind < test_nrdata;ind++){ //for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ // for(unsigned int src_h = 0; src_h < src_height;src_h++){ @@ -3286,602 +3365,602 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * } } */ - for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){ - unsigned int src_w = testw[ind]; - unsigned int src_h = testh[ind]; + for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){ + unsigned int src_w = testw[ind]; + unsigned int src_h = testh[ind]; - int src_ind = src_h*src_width+src_w; + int src_ind = src_h*src_width+src_w; if(src_maskdata[src_ind] == 255){ - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - //if(src_z > 0){ - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + //if(src_z > 0){ + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - double info = (src_z*src_z+dst_z*dst_z); + double info = (src_z*src_z+dst_z*dst_z); - //double d = mysign(dst_z-tz)*fabs(dst_nx*(dst_x-tx) + dst_ny*(dst_y-ty) + dst_nz*(dst_z-tz)); - double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - residuals.push_back(d/info); + //double d = mysign(dst_z-tz)*fabs(dst_nx*(dst_x-tx) + dst_ny*(dst_y-ty) + dst_nz*(dst_z-tz)); + double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + residuals.push_back(d/info); - //float diff_z = (dst_z-tz)/(src_z*src_z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 - //residuals.push_back(diff_z); + //float diff_z = (dst_z-tz)/(src_z*src_z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 + //residuals.push_back(diff_z); -// float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; -// float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - //float angle = (tnx*dst_x+tny*dst_y+tnz*dst_z)/sqrt(dst_x*dst_x + dst_y*dst_y + dst_z*dst_z); - //weights.push_back(1-angle); - //weights.push_back(ny*dst_ny); + // float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + // float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + //float angle = (tnx*dst_x+tny*dst_y+tnz*dst_z)/sqrt(dst_x*dst_x + dst_y*dst_y + dst_z*dst_z); + //weights.push_back(1-angle); + //weights.push_back(ny*dst_ny); - double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); - double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); + double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); + double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); - double dist_src = sqrt(src_x*src_x+src_y*src_y+src_z*src_z); - double angle_src = fabs((src_x*src_nx+src_y*src_ny+src_z*src_nz)/dist_src); - weights.push_back(angle_src * angle_dst); + double dist_src = sqrt(src_x*src_x+src_y*src_y+src_z*src_z); + double angle_src = fabs((src_x*src_nx+src_y*src_ny+src_z*src_nz)/dist_src); + weights.push_back(angle_src * angle_dst); - if(debugg){ - ws.push_back(src_w); - hs.push_back(src_h); + if(debugg){ + ws.push_back(src_w); + hs.push_back(src_h); - dst_ws.push_back(dst_w); - dst_hs.push_back(dst_h); - } - } - } - } - } - } + dst_ws.push_back(dst_w); + dst_hs.push_back(dst_h); + } + } + } + } + } + } -// DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); -// func->maxp = 1.0; -// func->update_size = true; -// func->zeromean = true; -// func->startreg = 0.0001; -// func->debugg_print = debugg; -// func->bidir = true; -// func->maxnoise = pred; -// func->reset(); + // DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + // func->maxp = 1.0; + // func->update_size = true; + // func->zeromean = true; + // func->startreg = 0.0001; + // func->debugg_print = debugg; + // func->bidir = true; + // func->maxnoise = pred; + // func->reset(); - DistanceWeightFunction2 * func = new DistanceWeightFunction2(); - func->f = THRESHOLD; + DistanceWeightFunction2 * func = new DistanceWeightFunction2(); + func->f = THRESHOLD; func->p = 0.01; - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} - func->computeModel(X); - - Eigen::VectorXd W = func->getProbs(X); - - delete func; - - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - oc.score += weight*weights.at(i); - oc.occlusions += ocl*weights.at(i); - } - - //debugg = true; - if(debugg){ - pcl::PointCloud::Ptr scloud (new pcl::PointCloud); - pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); - scloud->points.resize(src_width*src_height); - dcloud->points.resize(dst_width*dst_height); -/* - for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] != 255){continue;} - float z = src_idepth*float(src_depthdata[src_ind]); - if(z > 0){ - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - scloud->points[src_ind].x = tx; - scloud->points[src_ind].y = ty; - scloud->points[src_ind].z = tz+2; - scloud->points[src_ind].r = 0; - scloud->points[src_ind].g = 0; - scloud->points[src_ind].b = 255; - } - } - } - for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + func->computeModel(X); + + Eigen::VectorXd W = func->getProbs(X); + + delete func; + + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + oc.score += weight*weights.at(i); + oc.occlusions += ocl*weights.at(i); + } + + //debugg = true; + if(debugg){ + pcl::PointCloud::Ptr scloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + scloud->points.resize(src_width*src_height); + dcloud->points.resize(dst_width*dst_height); + /* + for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] != 255){continue;} + float z = src_idepth*float(src_depthdata[src_ind]); + if(z > 0){ + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + scloud->points[src_ind].x = tx; + scloud->points[src_ind].y = ty; + scloud->points[src_ind].z = tz+2; + scloud->points[src_ind].r = 0; + scloud->points[src_ind].g = 0; + scloud->points[src_ind].b = 255; + } + } + } + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ unsigned int dst_ind = dst_h*dst_width+dst_w; - if(true || dst_maskdata[dst_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ - float z = dst_idepth*float(dst_depthdata[dst_ind]); - if(z > 0){// && (dst_w%3 == 0) && (dst_h%3 == 0)){ - float x = (float(dst_w) - dst_cx) * z * dst_ifx; - float y = (float(dst_h) - dst_cy) * z * dst_ify; - dcloud->points[dst_ind].x = x; - dcloud->points[dst_ind].y = y; - dcloud->points[dst_ind].z = z+2; - dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; - dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; - dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; - if(dst_maskdata[dst_ind] == 255){ - dcloud->points[dst_ind].r = 255; - dcloud->points[dst_ind].g = 000; - dcloud->points[dst_ind].b = 255; - } - } - } - } - } - viewer->removeAllPointClouds(); - //printf("%i showing results\n",__LINE__); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - viewer->spin(); - viewer->removeAllPointClouds(); - */ -/* - for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - int dst_ind = dst_h*dst_width+dst_w; - dcloud->points[dst_ind].x = 0; - dcloud->points[dst_ind].y = 0; - dcloud->points[dst_ind].z = 0; - } - } - - - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - if(debugg){ - int w = ws[i]; - int h = hs[i]; - unsigned int src_ind = h * src_width + w; - if(ocl > 0.01 || weight > 0.01){ - scloud->points[src_ind].r = 255.0*ocl; - scloud->points[src_ind].g = 0; - scloud->points[src_ind].b = 0; - }else{ - scloud->points[src_ind].x = 0; - scloud->points[src_ind].y = 0; - scloud->points[src_ind].z = 0; - } - } - } - - viewer->removeAllPointClouds(); - //printf("%i showing results\n",__LINE__); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - viewer->spin(); - viewer->removeAllPointClouds(); - - for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] != 255){continue;} - float z = src_idepth*float(src_depthdata[src_ind]); - if(z > 0){ - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - scloud->points[src_ind].x = tx; - scloud->points[src_ind].y = ty; - scloud->points[src_ind].z = tz+2; - scloud->points[src_ind].r = 0; - scloud->points[src_ind].g = 0; - scloud->points[src_ind].b = 255; - } - } - } - for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - int dst_ind = dst_h*dst_width+dst_w; - if(true || dst_maskdata[dst_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ - float z = dst_idepth*float(dst_depthdata[dst_ind]); - if(z > 0){// && (dst_w%3 == 0) && (dst_h%3 == 0)){ - float x = (float(dst_w) - dst_cx) * z * dst_ifx; - float y = (float(dst_h) - dst_cy) * z * dst_ify; - dcloud->points[dst_ind].x = x; - dcloud->points[dst_ind].y = y; - dcloud->points[dst_ind].z = z+2; - dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; - dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; - dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; - if(dst_maskdata[dst_ind] == 255){ - dcloud->points[dst_ind].r = 255; - dcloud->points[dst_ind].g = 000; - dcloud->points[dst_ind].b = 255; - } - } - } - } - } - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - if(debugg){ - int w = ws[i]; - int h = hs[i]; - unsigned int src_ind = h * src_width + w; - if(ocl > 0.01 || weight > 0.01){ - scloud->points[src_ind].r = 255.0*ocl;//*weights.at(i); - scloud->points[src_ind].g = 255.0*weight;//*weights.at(i); - scloud->points[src_ind].b = 0; - }else{ - scloud->points[src_ind].x = 0; - scloud->points[src_ind].y = 0; - scloud->points[src_ind].z = 0; - } - } - } - - viewer->removeAllPointClouds(); - - //printf("%i showing results\n",__LINE__); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - viewer->spin(); - viewer->removeAllPointClouds(); + if(true || dst_maskdata[dst_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ + float z = dst_idepth*float(dst_depthdata[dst_ind]); + if(z > 0){// && (dst_w%3 == 0) && (dst_h%3 == 0)){ + float x = (float(dst_w) - dst_cx) * z * dst_ifx; + float y = (float(dst_h) - dst_cy) * z * dst_ify; + dcloud->points[dst_ind].x = x; + dcloud->points[dst_ind].y = y; + dcloud->points[dst_ind].z = z+2; + dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; + dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; + dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; + if(dst_maskdata[dst_ind] == 255){ + dcloud->points[dst_ind].r = 255; + dcloud->points[dst_ind].g = 000; + dcloud->points[dst_ind].b = 255; + } + } + } + } + } + viewer->removeAllPointClouds(); + //printf("%i showing results\n",__LINE__); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + */ + /* + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; + dcloud->points[dst_ind].x = 0; + dcloud->points[dst_ind].y = 0; + dcloud->points[dst_ind].z = 0; + } + } + + + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + int w = ws[i]; + int h = hs[i]; + unsigned int src_ind = h * src_width + w; + if(ocl > 0.01 || weight > 0.01){ + scloud->points[src_ind].r = 255.0*ocl; + scloud->points[src_ind].g = 0; + scloud->points[src_ind].b = 0; + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + //printf("%i showing results\n",__LINE__); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + + for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] != 255){continue;} + float z = src_idepth*float(src_depthdata[src_ind]); + if(z > 0){ + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + scloud->points[src_ind].x = tx; + scloud->points[src_ind].y = ty; + scloud->points[src_ind].z = tz+2; + scloud->points[src_ind].r = 0; + scloud->points[src_ind].g = 0; + scloud->points[src_ind].b = 255; + } + } + } + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; + if(true || dst_maskdata[dst_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ + float z = dst_idepth*float(dst_depthdata[dst_ind]); + if(z > 0){// && (dst_w%3 == 0) && (dst_h%3 == 0)){ + float x = (float(dst_w) - dst_cx) * z * dst_ifx; + float y = (float(dst_h) - dst_cy) * z * dst_ify; + dcloud->points[dst_ind].x = x; + dcloud->points[dst_ind].y = y; + dcloud->points[dst_ind].z = z+2; + dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; + dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; + dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; + if(dst_maskdata[dst_ind] == 255){ + dcloud->points[dst_ind].r = 255; + dcloud->points[dst_ind].g = 000; + dcloud->points[dst_ind].b = 255; + } + } + } + } + } + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + int w = ws[i]; + int h = hs[i]; + unsigned int src_ind = h * src_width + w; + if(ocl > 0.01 || weight > 0.01){ + scloud->points[src_ind].r = 255.0*ocl;//*weights.at(i); + scloud->points[src_ind].g = 255.0*weight;//*weights.at(i); + scloud->points[src_ind].b = 0; + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + + //printf("%i showing results\n",__LINE__); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); */ -/* - for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] != 255){continue;} - float z = src_idepth*float(src_depthdata[src_ind]); - if(z > 0){ - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; - scloud->points[src_ind].x = x; - scloud->points[src_ind].y = y; - scloud->points[src_ind].z = z; - scloud->points[src_ind].r = 0; - scloud->points[src_ind].g = 0; - scloud->points[src_ind].b = 0; - } - } - } + /* + for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] != 255){continue;} + float z = src_idepth*float(src_depthdata[src_ind]); + if(z > 0){ + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + scloud->points[src_ind].x = x; + scloud->points[src_ind].y = y; + scloud->points[src_ind].z = z; + scloud->points[src_ind].r = 0; + scloud->points[src_ind].g = 0; + scloud->points[src_ind].b = 0; + } + } + } */ - for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - unsigned int dst_ind = dst_h*dst_width+dst_w; - float z = dst_idepth*float(dst_depthdata[dst_ind]); - if(z > 0){ - float x = (float(dst_w) - dst_cx) * z * dst_ifx; - float y = (float(dst_h) - dst_cy) * z * dst_ify; - dcloud->points[dst_ind].x = x; - dcloud->points[dst_ind].y = y; - dcloud->points[dst_ind].z = z+2; - dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; - dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; - dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; - if(false && dst_maskdata[dst_ind] == 255){ - dcloud->points[dst_ind].r = 255; - dcloud->points[dst_ind].g = 000; - dcloud->points[dst_ind].b = 255; - } - } - } - } -// viewer->removeAllPointClouds(); -// //printf("%i showing results\n",__LINE__); -// viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); -// viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - - viewer->removeAllShapes(); - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - if(debugg){ - int w = ws[i]; - int h = hs[i]; - unsigned int src_ind = h * src_width + w; - - int dst_w = dst_ws[i]; - int dst_h = dst_hs[i]; - unsigned int dst_ind = dst_h * src_width + dst_w; - if(ocl > 0.01 || weight > 0.01){ - float z = src_idepth*float(src_depthdata[src_ind]); - float x = (float(w) - src_cx) * z * src_ifx; - float y = (float(h) - src_cy) * z * src_ify; - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - scloud->points[src_ind].x = tx; - scloud->points[src_ind].y = ty; - scloud->points[src_ind].z = tz+2; - - scloud->points[src_ind].r = 255.0*ocl*weights.at(i); - scloud->points[src_ind].g = 255.0*weight*weights.at(i); - scloud->points[src_ind].b = 0; - - char buf [1024]; - sprintf(buf,"line%i",i); - viewer->addLine (scloud->points[src_ind], dcloud->points[dst_ind],buf); - }else{ - scloud->points[src_ind].x = 0; - scloud->points[src_ind].y = 0; - scloud->points[src_ind].z = 0; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud"); - - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - viewer->spin(); - viewer->removeAllPointClouds(); - -/* - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - if(debugg){ - int w = ws[i]; - int h = hs[i]; - unsigned int src_ind = h * src_width + w; - if(ocl > 0.01 || weight > 0.01){ - scloud->points[src_ind].r = 255.0*(1-weights.at(i)); - scloud->points[src_ind].g = 255.0*weights.at(i); - scloud->points[src_ind].b = 0; - }else{ - scloud->points[src_ind].x = 0; - scloud->points[src_ind].y = 0; - scloud->points[src_ind].z = 0; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->spin(); - viewer->removeAllPointClouds(); + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + unsigned int dst_ind = dst_h*dst_width+dst_w; + float z = dst_idepth*float(dst_depthdata[dst_ind]); + if(z > 0){ + float x = (float(dst_w) - dst_cx) * z * dst_ifx; + float y = (float(dst_h) - dst_cy) * z * dst_ify; + dcloud->points[dst_ind].x = x; + dcloud->points[dst_ind].y = y; + dcloud->points[dst_ind].z = z+2; + dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; + dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; + dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; + if(false && dst_maskdata[dst_ind] == 255){ + dcloud->points[dst_ind].r = 255; + dcloud->points[dst_ind].g = 000; + dcloud->points[dst_ind].b = 255; + } + } + } + } + // viewer->removeAllPointClouds(); + // //printf("%i showing results\n",__LINE__); + // viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + // viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + + viewer->removeAllShapes(); + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + int w = ws[i]; + int h = hs[i]; + unsigned int src_ind = h * src_width + w; + + int dst_w = dst_ws[i]; + int dst_h = dst_hs[i]; + unsigned int dst_ind = dst_h * src_width + dst_w; + if(ocl > 0.01 || weight > 0.01){ + float z = src_idepth*float(src_depthdata[src_ind]); + float x = (float(w) - src_cx) * z * src_ifx; + float y = (float(h) - src_cy) * z * src_ify; + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + scloud->points[src_ind].x = tx; + scloud->points[src_ind].y = ty; + scloud->points[src_ind].z = tz+2; + + scloud->points[src_ind].r = 255.0*ocl*weights.at(i); + scloud->points[src_ind].g = 255.0*weight*weights.at(i); + scloud->points[src_ind].b = 0; + + char buf [1024]; + sprintf(buf,"line%i",i); + viewer->addLine (scloud->points[src_ind], dcloud->points[dst_ind],buf); + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud"); + + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + + /* + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + int w = ws[i]; + int h = hs[i]; + unsigned int src_ind = h * src_width + w; + if(ocl > 0.01 || weight > 0.01){ + scloud->points[src_ind].r = 255.0*(1-weights.at(i)); + scloud->points[src_ind].g = 255.0*weights.at(i); + scloud->points[src_ind].b = 0; + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->spin(); + viewer->removeAllPointClouds(); */ - } -/* - if(debugg){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - cv::namedWindow("debuggimage", cv::WINDOW_AUTOSIZE ); - cv::imshow( "debuggimage", debugg_img ); - //cv::waitKey(30); - viewer->spin(); - viewer->removeAllPointClouds(); - } - */ - return oc; + } + /* + if(debugg){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + cv::namedWindow("debuggimage", cv::WINDOW_AUTOSIZE ); + cv::imshow( "debuggimage", debugg_img ); + //cv::waitKey(30); + viewer->spin(); + viewer->removeAllPointClouds(); + } + */ + return oc; } using namespace std; using namespace Eigen; vector > ModelUpdater::computeAllOcclusionScores(RGBDFrame * src, cv::Mat src_mask, RGBDFrame * dst, cv::Mat dst_mask,Eigen::Matrix4d p, bool debugg){ - unsigned char * src_maskdata = (unsigned char *)(src_mask.data); - unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(src->depth.data); - float * src_normalsdata = (float *)(src->normals.data); - - unsigned char * dst_maskdata = (unsigned char *)(dst_mask.data); - unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); - float * dst_normalsdata = (float *)(dst->normals.data); - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - Camera * src_camera = src->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - const int * src_labels = src->labels; - const int src_nr_labels = src->nr_labels; - - Camera * dst_camera = dst->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - const int * dst_labels = dst->labels; - const int dst_nr_labels = src->nr_labels; - - vector< vector< OcclusionScore > > all_scores; - all_scores.resize(src_nr_labels); - for(int i = 0; i < src_nr_labels; i++){ - all_scores[i].resize(dst_nr_labels); - } - - std::vector< std::vector< std::vector > > all_residuals; - all_residuals.resize(src_nr_labels); - for(int i = 0; i < src_nr_labels; i++){ - all_residuals[i].resize(dst_nr_labels); - } - - for(unsigned int src_w = 0; src_w < src_width; src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ - float z = src_idepth*float(src_depthdata[src_ind]); - float nx = src_normalsdata[3*src_ind+0]; - - if(z > 0 && nx != 2){ - float ny = src_normalsdata[3*src_ind+1]; - float nz = src_normalsdata[3*src_ind+2]; - - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; - - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - - float tnx = m00*nx + m01*ny + m02*nz; - float tny = m10*nx + m11*ny + m12*nz; - float tnz = m20*nx + m21*ny + m22*nz; - - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - if(dst_z > 0){ - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - float diff_z2 = tnx*(dst_x-tx)+tny*(dst_y-ty)+tnz*(dst_z-tz); - - float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 - if(diff_z < 0 && diff_z2 > 0){diff_z2 *= -1;} - if(diff_z > 0 && diff_z2 < 0){diff_z2 *= -1;} - diff_z2 /= (z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 - - int src_label = src_labels[src_ind]; - int dst_label = dst_labels[dst_ind]; - all_residuals[src_label][dst_label].push_back(diff_z2); - } - } - } - } - } - } - - DistanceWeightFunction2PPR * func = new DistanceWeightFunction2PPR(); - func->update_size = true; - func->startreg = 0.00001; - func->debugg_print = true; - func->reset(); - - - delete func; - - for(int i = 0; i < src_nr_labels; i++){ - for(int j = 0; j < dst_nr_labels; i++){ - std::vector & resi = all_residuals[i][j]; - OcclusionScore score; - for(unsigned int k = 0; k < resi.size(); k++){ - float r = resi[k]; - float weight = 1; - if(fabs(r) > 0.0005){weight = 0;}//Replace with PPR - - float ocl = 0; - if(r > 0){ocl += 1-weight;} - - score.score += weight; - score.occlusions += ocl; - } - all_scores[i][j] = score; - } - } - return all_scores; + unsigned char * src_maskdata = (unsigned char *)(src_mask.data); + unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(src->depth.data); + float * src_normalsdata = (float *)(src->normals.data); + + unsigned char * dst_maskdata = (unsigned char *)(dst_mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); + float * dst_normalsdata = (float *)(dst->normals.data); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + Camera * src_camera = src->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + const int * src_labels = src->labels; + const int src_nr_labels = src->nr_labels; + + Camera * dst_camera = dst->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + const int * dst_labels = dst->labels; + const int dst_nr_labels = src->nr_labels; + + vector< vector< OcclusionScore > > all_scores; + all_scores.resize(src_nr_labels); + for(int i = 0; i < src_nr_labels; i++){ + all_scores[i].resize(dst_nr_labels); + } + + std::vector< std::vector< std::vector > > all_residuals; + all_residuals.resize(src_nr_labels); + for(int i = 0; i < src_nr_labels; i++){ + all_residuals[i].resize(dst_nr_labels); + } + + for(unsigned int src_w = 0; src_w < src_width; src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ + float z = src_idepth*float(src_depthdata[src_ind]); + float nx = src_normalsdata[3*src_ind+0]; + + if(z > 0 && nx != 2){ + float ny = src_normalsdata[3*src_ind+1]; + float nz = src_normalsdata[3*src_ind+2]; + + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + float tnx = m00*nx + m01*ny + m02*nz; + float tny = m10*nx + m11*ny + m12*nz; + float tnz = m20*nx + m21*ny + m22*nz; + + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + if(dst_z > 0){ + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + float diff_z2 = tnx*(dst_x-tx)+tny*(dst_y-ty)+tnz*(dst_z-tz); + + float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 + if(diff_z < 0 && diff_z2 > 0){diff_z2 *= -1;} + if(diff_z > 0 && diff_z2 < 0){diff_z2 *= -1;} + diff_z2 /= (z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 + + int src_label = src_labels[src_ind]; + int dst_label = dst_labels[dst_ind]; + all_residuals[src_label][dst_label].push_back(diff_z2); + } + } + } + } + } + } + + DistanceWeightFunction2PPR * func = new DistanceWeightFunction2PPR(); + func->update_size = true; + func->startreg = 0.00001; + func->debugg_print = true; + func->reset(); + + + delete func; + + for(int i = 0; i < src_nr_labels; i++){ + for(int j = 0; j < dst_nr_labels; i++){ + std::vector & resi = all_residuals[i][j]; + OcclusionScore score; + for(unsigned int k = 0; k < resi.size(); k++){ + float r = resi[k]; + float weight = 1; + if(fabs(r) > 0.0005){weight = 0;}//Replace with PPR + + float ocl = 0; + if(r > 0){ocl += 1-weight;} + + score.score += weight; + score.occlusions += ocl; + } + all_scores[i][j] = score; + } + } + return all_scores; } vector > ModelUpdater::getOcclusionScores(std::vector current_poses, std::vector current_frames, std::vector current_modelmasks, bool debugg_scores, double speedup){ - //printf("getOcclusionScores\n"); + //printf("getOcclusionScores\n"); - long total_points = 0; - for(unsigned int i = 0; i < current_frames.size(); i++){total_points+=current_modelmasks[i]->testw.size();} - int step = std::max(long(1),long(speedup*total_points*long(current_frames.size()))/long(50000000)); + long total_points = 0; + for(unsigned int i = 0; i < current_frames.size(); i++){total_points+=current_modelmasks[i]->testw.size();} + int step = std::max(long(1),long(speedup*total_points*long(current_frames.size()))/long(50000000)); -// printf("total_points: %i\n",total_points); -// printf("current_frames.size(): %i\n",current_frames.size()); -// printf("ratio: %f\n",double(total_points*long(current_frames.size()))/double(50000000)); - printf("step: %i\n",step); + // printf("total_points: %i\n",total_points); + // printf("current_frames.size(): %i\n",current_frames.size()); + // printf("ratio: %f\n",double(total_points*long(current_frames.size()))/double(50000000)); + printf("step: %i\n",step); - vector > occlusionScores; - occlusionScores.resize(current_frames.size()); + vector > occlusionScores; + occlusionScores.resize(current_frames.size()); for(unsigned int i = 0; i < current_frames.size(); i++){occlusionScores[i].resize(current_frames.size());} - int max_points = step;//100000.0/double(current_frames.size()*(current_frames.size()-1)); + int max_points = step;//100000.0/double(current_frames.size()*(current_frames.size()-1)); //float occlusion_penalty = 10.0f; - std::vector > scores; - scores.resize(occlusionScores.size()); + std::vector > scores; + scores.resize(occlusionScores.size()); for(unsigned int i = 0; i < occlusionScores.size(); i++){scores[i].resize(occlusionScores.size());} bool lock = false; for(unsigned int i = 0; i < current_frames.size(); i++){ - scores[i][i] = 0; + scores[i][i] = 0; for(unsigned int j = i+1; j < current_frames.size(); j++){ - //printf("scores %i %i\n",i,j); - if(lock && current_modelmasks[j]->sweepid == current_modelmasks[i]->sweepid && current_modelmasks[j]->sweepid != -1){ - occlusionScores[i][j].score = 999999; - occlusionScores[i][j].occlusions = 0; - occlusionScores[j][i].score = 999999; - occlusionScores[j][i].occlusions = 0; - }else{ - Eigen::Matrix4d relative_pose = current_poses[i].inverse() * current_poses[j]; - occlusionScores[j][i] = computeOcclusionScore(current_frames[j], current_modelmasks[j],current_frames[i], current_modelmasks[i], relative_pose,max_points,debugg_scores); - occlusionScores[i][j] = computeOcclusionScore(current_frames[i], current_modelmasks[i],current_frames[j], current_modelmasks[j], relative_pose.inverse(),max_points,debugg_scores); - //printf("scores: %i %i -> occlusion_penalty: %f -> (%f %f) and (%f %f) -> %f \n",i,j,occlusion_penalty,occlusionScores[i][j].score,occlusionScores[i][j].occlusions,occlusionScores[j][i].score,occlusionScores[j][i].occlusions,occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions)); - } - scores[i][j] = occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions); - scores[j][i] = scores[i][j]; - } - } - return occlusionScores; + //printf("scores %i %i\n",i,j); + if(lock && current_modelmasks[j]->sweepid == current_modelmasks[i]->sweepid && current_modelmasks[j]->sweepid != -1){ + occlusionScores[i][j].score = 999999; + occlusionScores[i][j].occlusions = 0; + occlusionScores[j][i].score = 999999; + occlusionScores[j][i].occlusions = 0; + }else{ + Eigen::Matrix4d relative_pose = current_poses[i].inverse() * current_poses[j]; + occlusionScores[j][i] = computeOcclusionScore(current_frames[j], current_modelmasks[j],current_frames[i], current_modelmasks[i], relative_pose,max_points,debugg_scores); + occlusionScores[i][j] = computeOcclusionScore(current_frames[i], current_modelmasks[i],current_frames[j], current_modelmasks[j], relative_pose.inverse(),max_points,debugg_scores); + //printf("scores: %i %i -> occlusion_penalty: %f -> (%f %f) and (%f %f) -> %f \n",i,j,occlusion_penalty,occlusionScores[i][j].score,occlusionScores[i][j].occlusions,occlusionScores[j][i].score,occlusionScores[j][i].occlusions,occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions)); + } + scores[i][j] = occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions); + scores[j][i] = scores[i][j]; + } + } + return occlusionScores; } CloudData * ModelUpdater::getCD(std::vector current_poses, std::vector current_frames,std::vector current_masks, int step){return 0;} @@ -3889,17 +3968,17 @@ CloudData * ModelUpdater::getCD(std::vector current_poses, std: void ModelUpdater::computeMassRegistration(std::vector current_poses, std::vector current_frames,std::vector current_masks){} std::vector > ModelUpdater::getScores(std::vector > occlusionScores){//, float occlusion_penalty){ - std::vector > scores; - scores.resize(occlusionScores.size()); + std::vector > scores; + scores.resize(occlusionScores.size()); for(unsigned int i = 0; i < occlusionScores.size(); i++){scores[i].resize(occlusionScores.size());} for(unsigned int i = 0; i < scores.size(); i++){ - scores[i][i] = 0; + scores[i][i] = 0; for(unsigned int j = i+1; j < scores.size(); j++){ - scores[i][j] = occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions); - scores[j][i] = scores[i][j]; - } - } - return scores; + scores[i][j] = occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions); + scores[j][i] = scores[i][j]; + } + } + return scores; } } diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp index cfc149e3..cc2344ba 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp @@ -82,6 +82,7 @@ Eigen::MatrixXd MassRegistration::getMat(int rows, int cols, double * datas){ void MassRegistration::show(std::vector Xv, bool save, std::string filename, bool stop){ + printf("show\n"); //for(unsigned int a = 0; a < Xv.size(); a++){ viewer->removeAllPointClouds(); @@ -117,12 +118,17 @@ void MassRegistration::show(std::vector Xv, bool save, std::str p.r = b; cloud->points.push_back(p); } + + printf("cloud->points: %i\n",cloud->points.size()); + char buf [1024]; sprintf(buf,"cloud%i",xi); viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); } + + if(!save){ viewer->spin(); }else{ diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 20375157..7ed43bab 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -3112,6 +3112,7 @@ if(false){ } void MassRegistrationPPR2::showEdges(std::vector poses){ + printf("showEdges\n"); pcl::PointCloud::Ptr cloud (new pcl::PointCloud); for(unsigned int i = 0; i < poses.size(); i++){ printf("%i/%i -> %i\n",i,poses.size(),depthedge_nr_arraypoints[i]); @@ -3142,9 +3143,11 @@ void MassRegistrationPPR2::showEdges(std::vector poses){ cloud->points.push_back(p); } } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); + if(cloud->points.size() > 0){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } } MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses){ From 28238765674e111cf442512aa38430ce4ee90c7a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 22 Aug 2016 17:11:56 +0200 Subject: [PATCH 072/255] segmentation improved! --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 6 +- .../metaroom_additional_view_processing.cpp | 383 +++++++++---- .../quasimodo_models/include/core/RGBDFrame.h | 3 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 505 +++++++++++++++--- .../src/modelupdater/ModelUpdater.cpp | 264 ++++++--- 5 files changed, 922 insertions(+), 239 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index cd507903..5e751519 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -225,7 +225,7 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); massregmod->timeout = 1200; massregmod->viewer = viewer; - massregmod->visualizationLvl = 1; + massregmod->visualizationLvl = 0; massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massregmod->nomask = true; @@ -394,8 +394,8 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec //exit(0); // std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,mod_po_vec[j],mod_fr_vec[j],masks,mod_po_vec[j],mod_fr_vec[j],masks,true);//Determine self occlusions // std::vector external_masks = mu->computeDynamicObject(mod_po_vec[j],mod_fr_vec[j],masks,bgcp,bgcf,bgmask,mod_po_vec[j],mod_fr_vec[j],masks,true);//Determine external occlusions - std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions - std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,false);//Determine external occlusions + std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions + std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,false);//Determine external occlusions std::vector dynamic_masks; // for(unsigned int i = 0; i < mod_fr_vec[j].size(); i++){ // reglib::RGBDFrame * frame = mod_fr_vec[j][i]; diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 53c4c0e4..03fddff0 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -116,12 +116,21 @@ int visualization_lvl = 0; std::string outtopic = "/some/topic"; ros::Publisher out_pub; +void remove_old_seg(std::string sweep_folder){ + char buf [1024]; + sprintf(buf,"rm %s/dynamic_*.png",sweep_folder.c_str()); + printf("%s\n",buf); + + sprintf(buf,"rm %s/moving_*.png",sweep_folder.c_str()); + printf("%s\n",buf); +} + void processMetaroom(std::string path){ printf("processing: %s\n",path.c_str()); int slash_pos = path.find_last_of("/"); std::string sweep_folder = path.substr(0, slash_pos) + "/"; - +remove_old_seg(sweep_folder); std::vector viewrgbs; std::vector viewdepths; std::vector viewtfs; @@ -130,10 +139,8 @@ void processMetaroom(std::string path){ for (auto objectFile : objectFiles){ auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); for (unsigned int i=0; iheight,cloud->width,CV_8UC3); unsigned char * rgbdata = (unsigned char *)rgb.data; @@ -161,7 +168,6 @@ void processMetaroom(std::string path){ cv::namedWindow("depthimage", cv::WINDOW_AUTOSIZE); cv::imshow( "depthimage", depth); cv::waitKey(30); - } } @@ -213,15 +219,13 @@ void processMetaroom(std::string path){ sweep->points = mu->getSuperPoints(sweep->relativeposes,sweep->frames,sweep->modelmasks,1,false); - - //Not needed if metaroom well calibrated reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); bgmassreg->timeout = 20; bgmassreg->viewer = viewer; bgmassreg->use_surface = true; bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = 0;//visualization_lvl; + bgmassreg->visualizationLvl = 0;//visualization_lvl; bgmassreg->maskstep = 15; bgmassreg->nomaskstep = 15; bgmassreg->nomask = true; @@ -230,44 +234,44 @@ void processMetaroom(std::string path){ bgmassreg->setData(frames,masks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); - reglib::Model * av = new reglib::Model(); - av->frames = frames; - av->modelmasks = masks; - for(unsigned int i = 0; i < frames.size(); i++){ - av->relativeposes.push_back(bgmfr.poses[1].inverse()*bgmfr.poses[i+1]); - } - - - av->points = mu->getSuperPoints(av->relativeposes,av->frames,av->modelmasks,1,false); - - for(unsigned int i = 0; i < frames.size(); i++){ - frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; - } - - -// reglib::Model * fullmodel = new reglib::Model(); -// fullmodel->submodels.push_back(sweep); -// fullmodel->submodels_relativeposes.push_back(bgmfr.poses[0]); -// if(frames.size() > 0){ -// fullmodel->submodels.push_back(av); -// fullmodel->submodels_relativeposes.push_back(bgmfr.poses[1]); -// } - - reglib::Model * fullmodel = new reglib::Model(); - fullmodel->frames = sweep->frames; - fullmodel->relativeposes = sweep->relativeposes; - fullmodel->modelmasks = sweep->modelmasks; - for(unsigned int i = 0; i < frames.size(); i++){ - fullmodel->frames.push_back(frames[i]); - fullmodel->modelmasks.push_back(masks[i]); - fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); - } - - std::vector po; - std::vector fr; - std::vector mm; - fullmodel->getData(po, fr, mm); - fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); + reglib::Model * av = new reglib::Model(); + av->frames = frames; + av->modelmasks = masks; + for(unsigned int i = 0; i < frames.size(); i++){ + av->relativeposes.push_back(bgmfr.poses[1].inverse()*bgmfr.poses[i+1]); + } + + + av->points = mu->getSuperPoints(av->relativeposes,av->frames,av->modelmasks,1,false); + + for(unsigned int i = 0; i < frames.size(); i++){ + frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; + } + + + // reglib::Model * fullmodel = new reglib::Model(); + // fullmodel->submodels.push_back(sweep); + // fullmodel->submodels_relativeposes.push_back(bgmfr.poses[0]); + // if(frames.size() > 0){ + // fullmodel->submodels.push_back(av); + // fullmodel->submodels_relativeposes.push_back(bgmfr.poses[1]); + // } + + reglib::Model * fullmodel = new reglib::Model(); + fullmodel->frames = sweep->frames; + fullmodel->relativeposes = sweep->relativeposes; + fullmodel->modelmasks = sweep->modelmasks; + for(unsigned int i = 0; i < frames.size(); i++){ + fullmodel->frames.push_back(frames[i]); + fullmodel->modelmasks.push_back(masks[i]); + fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); + } + + std::vector po; + std::vector fr; + std::vector mm; + fullmodel->getData(po, fr, mm); + fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); SimpleXMLParser parser; SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); @@ -293,29 +297,109 @@ void processMetaroom(std::string path){ } + QFile file((sweep_folder+"ViewGroup.xml").c_str()); + if (file.exists()){file.remove();} + + + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){ + std::cerr<<"Could not open file "<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("ViewGroup"); + + for(unsigned int i = 0; i < fullmodel->frames.size(); i++){ + char buf [1024]; + xmlWriter->writeStartElement("View"); +// sprintf(buf,"%s/RGB%10.10i.png",sweep_folder.c_str(),i); +// cv::imwrite(buf, fullmodel->frames[i]->rgb ); +// sprintf(buf,"%s/DEPTH%10.10i.png",sweep_folder.c_str(),i); +// cv::imwrite(buf, fullmodel->frames[i]->depth ); + +// xmlWriter->writeStartElement("AdditionalViewTransform"); + +// geometry_msgs::TransformStamped msg; +// tf::transformStampedTFToMsg(viewtfs[i], msg); + +// xmlWriter->writeStartElement("Stamp"); +// xmlWriter->writeStartElement("sec"); +// xmlWriter->writeCharacters(QString::number(msg.header.stamp.sec)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("nsec"); +// xmlWriter->writeCharacters(QString::number(msg.header.stamp.nsec)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeEndElement(); // Stamp + +// xmlWriter->writeStartElement("FrameId"); +// xmlWriter->writeCharacters(QString(msg.header.frame_id.c_str())); +// xmlWriter->writeEndElement(); + +// xmlWriter->writeStartElement("ChildFrameId"); +// xmlWriter->writeCharacters(QString(msg.child_frame_id.c_str())); +// xmlWriter->writeEndElement(); + +// xmlWriter->writeStartElement("Transform"); +// xmlWriter->writeStartElement("Translation"); +// xmlWriter->writeStartElement("x"); +// xmlWriter->writeCharacters(QString::number(msg.transform.translation.x)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("y"); +// xmlWriter->writeCharacters(QString::number(msg.transform.translation.y)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("z"); +// xmlWriter->writeCharacters(QString::number(msg.transform.translation.z)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeEndElement(); // Translation +// xmlWriter->writeStartElement("Rotation"); +// xmlWriter->writeStartElement("w"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("x"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("y"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeStartElement("z"); +// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z)); +// xmlWriter->writeEndElement(); +// xmlWriter->writeEndElement(); // Rotation +// xmlWriter->writeEndElement(); // Transform +// xmlWriter->writeEndElement(); // RoomIntermediateCloudTransform + xmlWriter->writeEndElement(); // View + } + xmlWriter->writeEndElement(); // ViewGroup + xmlWriter->writeEndDocument(); + delete xmlWriter; + +exit(0); if(prevind != -1){ std::string prev = sweep_xmls[prevind]; printf("prev: %s\n",prev.c_str()); reglib::Model * bg = quasimodo_brain::load_metaroom_model(prev); - bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); std::vector< reglib::Model * > models; - //models.push_back(sweep); - //models.push_back(fullmodel); - models.push_back(av); + //models.push_back(sweep); + models.push_back(fullmodel); + //models.push_back(av); std::vector< std::vector< cv::Mat > > internal; std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; - quasimodo_brain::segment(bg,models,internal,external,dynamic,true); + quasimodo_brain::segment(bg,models,internal,external,dynamic,true); - for(unsigned int i = 0; visualization_lvl > 0 && i < models.size(); i++){ + for(unsigned int i = 0; i < models.size(); i++){ std::vector internal_masks = internal[i]; std::vector external_masks = external[i]; std::vector dynamic_masks = dynamic[i]; @@ -327,11 +411,16 @@ void processMetaroom(std::string path){ std::vector mod_mm; model->getData(mod_po, mod_fr, mod_mm); + std::vector dynamic_frameid; + std::vector dynamic_pixelid; + + std::vector moving_frameid; + std::vector moving_pixelid; + + pcl::PointCloud::Ptr dynamiccloud (new pcl::PointCloud); + pcl::PointCloud::Ptr movingcloud (new pcl::PointCloud); pcl::PointCloud::Ptr cloud (new pcl::PointCloud); -// for(unsigned int j = 0; j < model->frames.size(); j++){ -// reglib::RGBDFrame * frame = model->frames[j]; -// Eigen::Matrix4d p = model->relativeposes[j]; for(unsigned int j = 0; j < mod_fr.size(); j++){ reglib::RGBDFrame * frame = mod_fr[j]; Eigen::Matrix4d p = mod_po[j]; @@ -345,7 +434,6 @@ void processMetaroom(std::string path){ unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); @@ -377,10 +465,16 @@ void processMetaroom(std::string path){ point.r = rgbdata[3*ind+2]; if(dynamicmaskdata[ind] != 0){ + dynamiccloud->points.push_back(point); + dynamic_frameid.push_back(j); + dynamic_pixelid.push_back(ind); point.b = 0; point.g = 255; point.r = 0; }else if(internalmaskdata[ind] == 0){ + movingcloud->points.push_back(point); + moving_frameid.push_back(j); + moving_pixelid.push_back(ind); point.b = 0; point.g = 0; point.r = 255; @@ -392,10 +486,101 @@ void processMetaroom(std::string path){ } } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - viewer->spin(); - //while(cv::waitKey(50)!='q'){viewer->spinOnce();} + // Creating the KdTree object for the search method of the extraction + pcl::search::KdTree::Ptr dynamictree (new pcl::search::KdTree); + dynamictree->setInputCloud (dynamiccloud); + + std::vector dynamic_indices; + pcl::EuclideanClusterExtraction dynamic_ec; + dynamic_ec.setClusterTolerance (0.02); // 2cm + dynamic_ec.setMinClusterSize (500); + dynamic_ec.setMaxClusterSize (250000000); + dynamic_ec.setSearchMethod (dynamictree); + dynamic_ec.setInputCloud (dynamiccloud); + dynamic_ec.extract (dynamic_indices); + + for (unsigned int d = 0; d < dynamic_indices.size(); d++){ + std::vector< std::vector > inds; + inds.resize(mod_fr.size()); + + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ + int pid = dynamic_indices[d].indices[ind]; + inds[dynamic_frameid[pid]].push_back(dynamic_pixelid[pid]); + cloud_cluster->points.push_back(dynamiccloud->points[pid]); + } + + for(unsigned int j = 0; j < mod_fr.size(); j++){ + if(inds[j].size() == 0){continue;} + reglib::RGBDFrame * frame = mod_fr[j]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 0;} + for(unsigned int k = 0; k < inds[j].size(); k++){maskdata[inds[j][k]] = 255;} + + char buf [1024]; + sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),d,j); + cv::imwrite(buf, mask ); + + cv::namedWindow( "mask", cv::WINDOW_AUTOSIZE ); cv::imshow( "mask", mask); + cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb); + cv::waitKey(0); + } + + if(visualization_lvl > 0){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "scloud"); + viewer->spin(); + } + } + + // Creating the KdTree object for the search method of the extraction + pcl::search::KdTree::Ptr movingtree (new pcl::search::KdTree); + movingtree->setInputCloud (movingcloud); + + std::vector moving_indices; + pcl::EuclideanClusterExtraction moving_ec; + moving_ec.setClusterTolerance (0.02); // 2cm + moving_ec.setMinClusterSize (500); + moving_ec.setMaxClusterSize (250000000); + moving_ec.setSearchMethod (movingtree); + moving_ec.setInputCloud (movingcloud); + moving_ec.extract (moving_indices); + + for (unsigned int d = 0; d < moving_indices.size(); d++){ + std::vector< std::vector > inds; + inds.resize(mod_fr.size()); + + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ + int pid = moving_indices[d].indices[ind]; + inds[moving_frameid[pid]].push_back(moving_pixelid[pid]); + cloud_cluster->points.push_back(movingcloud->points[pid]); + } + + if(visualization_lvl > 0){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "scloud"); + viewer->spin(); + } + } + + if(visualization_lvl > 0){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (dynamiccloud, pcl::visualization::PointCloudColorHandlerRGBField(dynamiccloud), "scloud"); + viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (movingcloud, pcl::visualization::PointCloudColorHandlerRGBField(movingcloud), "scloud"); + viewer->spin(); + } + } } @@ -406,30 +591,30 @@ void processMetaroom(std::string path){ delete reg; delete mu; -// xmlWriter->writeStartElement("Transform"); -// xmlWriter->writeStartElement("Translation"); -// xmlWriter->writeStartElement("x"); -// xmlWriter->writeCharacters(QString::number(msg.transform.translation.x)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeStartElement("y"); -// xmlWriter->writeCharacters(QString::number(msg.transform.translation.y)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeStartElement("z"); -// xmlWriter->writeCharacters(QString::number(msg.transform.translation.z)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeEndElement(); // Translation -// xmlWriter->writeStartElement("Rotation"); -// xmlWriter->writeStartElement("w"); -// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeStartElement("x"); -// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeStartElement("y"); -// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeStartElement("z"); -// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z)); + // xmlWriter->writeStartElement("Transform"); + // xmlWriter->writeStartElement("Translation"); + // xmlWriter->writeStartElement("x"); + // xmlWriter->writeCharacters(QString::number(msg.transform.translation.x)); + // xmlWriter->writeEndElement(); + // xmlWriter->writeStartElement("y"); + // xmlWriter->writeCharacters(QString::number(msg.transform.translation.y)); + // xmlWriter->writeEndElement(); + // xmlWriter->writeStartElement("z"); + // xmlWriter->writeCharacters(QString::number(msg.transform.translation.z)); + // xmlWriter->writeEndElement(); + // xmlWriter->writeEndElement(); // Translation + // xmlWriter->writeStartElement("Rotation"); + // xmlWriter->writeStartElement("w"); + // xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w)); + // xmlWriter->writeEndElement(); + // xmlWriter->writeStartElement("x"); + // xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x)); + // xmlWriter->writeEndElement(); + // xmlWriter->writeStartElement("y"); + // xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y)); + // xmlWriter->writeEndElement(); + // xmlWriter->writeStartElement("z"); + // xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z)); std_msgs::String msg; msg.data = path; @@ -443,7 +628,7 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg){ void segmentWithAdditionalViewsXml(std::string sweep_xml,std::string prev){ printf("segmentWithAdditionalViewsXml\n"); -// std::string prev = getPrevXML(sweep_xml); + // std::string prev = getPrevXML(sweep_xml); } int main(int argc, char** argv){ @@ -482,19 +667,19 @@ int main(int argc, char** argv){ } } /* - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - for(int ar = 1; ar < argc; ar++){ - string overall_folder = std::string(argv[ar]); - vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); - printf("sweep_xmls\n"); - for (auto sweep_xml : sweep_xmls) { - printf("sweep_xml: %s\n",sweep_xml.c_str()); - load2(sweep_xml); - } - } + for(int ar = 1; ar < argc; ar++){ + string overall_folder = std::string(argv[ar]); + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + printf("sweep_xmls\n"); + for (auto sweep_xml : sweep_xmls) { + printf("sweep_xml: %s\n",sweep_xml.c_str()); + load2(sweep_xml); + } + } @@ -518,11 +703,11 @@ int main(int argc, char** argv){ } //ros::spin(); - delete reg; - for(size_t j = 0; j < models.size(); j++){ - models[j]->fullDelete(); - delete models[j]; - } + delete reg; + for(size_t j = 0; j < models.size(); j++){ + models[j]->fullDelete(); + delete models[j]; + } printf("done\n"); */ return 0; diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 4670a3bb..3d662876 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -22,6 +22,7 @@ #include "Util.h" #include "Camera.h" +#include "../weightfunctions/DistanceWeightFunction2.h" namespace reglib { @@ -35,7 +36,7 @@ namespace reglib int sweepid; //float * rgbdata; - + cv::Mat det_dilate; cv::Mat rgb; cv::Mat depth; cv::Mat normals; diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 2517bb08..659fb9f3 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -81,6 +81,214 @@ float weightFunc(int w0, int h0, int w1, int h1, int width, std::vector return weight; } +std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ + cv::Mat src = frame->rgb.clone(); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + const float idepth = frame->camera->idepth_scale; + + cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); + + unsigned char * srcdata = (unsigned char *)src.data; + unsigned int width = src.cols; + unsigned int height = src.rows; + + std::vector< std::vector > probs; + unsigned int chans = 3; + for(unsigned int c = 0; c < chans;c++){ + std::vector Xvec; + int dir; + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + dir = 1; + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = width; + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + } + } + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + func->zeromean = true; + func->maxp = 0.99; + func->startreg = 0.0; + func->debugg_print = false; + func->bidir = true; + func->maxd = 256.0; + func->histogram_size = 256; + func->fixed_histogram_size = true; + func->startmaxd = func->maxd; + func->starthistogram_size = func->histogram_size; + func->blurval = 0.005; + func->stdval2 = stdval; + func->maxnoise = stdval; + func->reset(); + func->computeModel(X); + + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + std::vector dxy; dxy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dxy[i] = 0.5;} + std::vector dyx; dyx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dyx[i] = 0.5;} + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + + dir = 1; + dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = width; + dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + } + } + delete func; + + probs.push_back(dx); + probs.push_back(dy); + } + + { + std::vector Xvec; + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + } + } + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); + funcZ->zeromean = true; + funcZ->startreg = 0.002; + funcZ->debugg_print = false; + funcZ->bidir = true; + funcZ->maxp = 0.999999; + funcZ->maxd = 0.1; + funcZ->histogram_size = 100; + funcZ->fixed_histogram_size = true; + funcZ->startmaxd = funcZ->maxd; + funcZ->starthistogram_size = funcZ->histogram_size; + funcZ->blurval = 0.5; + funcZ->stdval2 = stdval; + funcZ->maxnoise = stdval; + funcZ->reset(); + funcZ->computeModel(X); + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + } + } + + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + + float dz = fabs(z-z2); + + if(z3 > 0){dz = std::min(float(dz),float(fabs(z- (2*z2-z3))));} + + if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + float dz = fabs(z-z2); + + if(z3 > 0){dz = std::min(float(dz),float(fabs(z- (2*z2-z3))));} + if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + } + } + } + + delete funcZ; + probs.push_back(dx); + probs.push_back(dy); + } + return probs; +} + RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals){ keyval = ""; @@ -135,92 +343,245 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt // rgbdata[i] = float(img_rgbdata[i]); // } - depthedges.create(height,width,CV_8UC1); - unsigned char * depthedgesdata = (unsigned char *)depthedges.data; - for(int i = 0; i < width*height; i++){ - depthedgesdata[i] = 0; - } - //rgbdata + //std::vector< std::vector > probs = getImageProbs(this,5); - /* - double t = 0.01; - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - depthedgesdata[ind] = 0; - double z = idepth*double(depthdata[ind]); - if(w > 0){ - double z2 = idepth*double(depthdata[ind-1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } + cv::Mat de; + de.create(height,width,CV_32FC3); + float * dedata = (float*)de.data; + for(int i = 0; i < 3*width*height; i++){ + dedata[i] = 0; + } - if(w < width-1){ - double z2 = idepth*double(depthdata[ind+1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } + cv::Mat ce; + ce.create(height,width,CV_32FC3); + float * cedata = (float*)ce.data; + for(int i = 0; i < 2*width*height; i++){ + cedata[i] = 0; + } - if(h > 0){ - double z2 = idepth*double(depthdata[ind-width]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } + std::vector Xvec; - if(h < height-1){ - double z2 = idepth*double(depthdata[ind+width]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); - if(h > 0 && w > 0){ - double z2 = idepth*double(depthdata[ind-width-1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; - if(w > 0 && h < height-1){ - double z2 = idepth*double(depthdata[ind+width-1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; - if(h > 0 && w < width-1){ - double z2 = idepth*double(depthdata[ind-width+1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } + if(z2 > 0 || z > 0){ + dedata[3*ind+1] = fabs((z-z2)/(z*z+z2*z2)); + Xvec.push_back(dedata[3*ind+1]); + } + } - if(h < height-1 && w < width-1){ - double z2 = idepth*double(depthdata[ind+width+1]); - double info = 1.0/(z*z+z2*z2); - double diff = fabs(z2-z)*info; - if(diff > t){depthedgesdata[ind] = 255;} - } - } - } + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){ + dedata[3*ind+2] = fabs((z-z2)/(z*z+z2*z2)); + Xvec.push_back(dedata[3*ind+2]); + } + } + } + } + + + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); + funcZ->zeromean = true; + funcZ->startreg = 0.002; + funcZ->debugg_print = false; + funcZ->bidir = true; + funcZ->maxp = 0.999999; + funcZ->maxd = 0.1; + funcZ->histogram_size = 100; + funcZ->fixed_histogram_size = true; + funcZ->startmaxd = funcZ->maxd; + funcZ->starthistogram_size = funcZ->histogram_size; + funcZ->blurval = 0.5; + funcZ->stdval2 = stdval; + funcZ->maxnoise = stdval; + funcZ->reset(); + funcZ->computeModel(X); + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + if(w > 1){ + dedata[3*ind+1] = 1-funcZ->getProb(dedata[3*ind+1]); + } + + if(h > 1){ + dedata[3*ind+2] = 1-funcZ->getProb(dedata[3*ind+2]); + } + } + } + + delete funcZ; + + cv::Mat det; + det.create(height,width,CV_8UC1); + unsigned char * detdata = (unsigned char*)det.data; + for(int i = 0; i < width*height; i++){ + detdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5)); + } + + + + + int dilation_size = 3; + cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) ); +/* + cv::Mat det2; + det2.create(height,width,CV_8UC3); + unsigned char * detdata2 = (unsigned char*)det2.data; + for(int i = 0; i < width*height; i++){ + detdata2[3*i+0] = detdata2[3*i+1] = detdata2[3*i+2] = detdata[i]; + } + + int area = 5; + for(unsigned int w = area; w < width-area;w++){ + for(unsigned int h = area; h < height-area;h++){ + bool p11 = detdata[h*width+w-0] != 0; + if(!p11){continue;} + + int sum = 0; + for(unsigned int x = w-area; x < w+area;x++){ + for(unsigned int y = h-area; y < h+area;y++){ + sum += detdata[y*width+x] != 0; + } + } + + if(sum <= area/2){ + cv::circle(det2, cv::Point(w,h), 5, cv::Scalar(0,255,0)); + } + + } + } + + + cv::namedWindow( "de", cv::WINDOW_AUTOSIZE ); cv::imshow( "de", de); + cv::namedWindow( "det", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det); + cv::namedWindow( "det2", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det2); + cv::namedWindow( "det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "det_dilate", det_dilate); + cv::namedWindow( "ce", cv::WINDOW_AUTOSIZE ); cv::imshow( "ce", ce); + cv::waitKey(0); */ +/* +{ + + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + } + } - // cv::Mat src = rgb.clone(); + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} - // for(int i = 0; i < 20; i++){ - // cv::Mat dst; - // cv::adaptiveBilateralFilter(src, dst, cv::Size(7,7), 30); + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); - // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); - // cv::namedWindow( "bilat", cv::WINDOW_AUTOSIZE ); cv::imshow( "bilat", dst); - // cv::waitKey(0); - // src = dst.clone(); - // } + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + + float dz = fabs(z-z2); + if(z3 > 0){dz = std::min(dz,float(fabs(z- (2*z2-z3))));} + if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + + float dz = fabs(z-z2); + if(z3 > 0){dz = std::min(dz,float(fabs(z- (2*z2-z3))));} + if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + } + } + } + + + + for(int i = 0; i < width*height; i++){ + dedata[3*i+0] = 1-dx[i]; + dedata[3*i+1] = 1-dy[i]; + dedata[2*i+2] = 0; + } + +} +*/ +// cv::namedWindow( "de", cv::WINDOW_AUTOSIZE ); cv::imshow( "de", de); +// cv::namedWindow( "ce", cv::WINDOW_AUTOSIZE ); cv::imshow( "ce", ce); +// cv::waitKey(0); + + + + depthedges.create(height,width,CV_8UC1); + unsigned char * depthedgesdata = (unsigned char *)depthedges.data; + for(int i = 0; i < width*height; i++){ + depthedgesdata[i] = 0; + } if(false){ pcl::PointCloud::Ptr cloud (new pcl::PointCloud); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 1073727c..2a079e32 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1597,6 +1597,9 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); float * src_normalsdata = (float *)(frame1->normals.data); + unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); + unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); + Camera * src_camera = frame1->camera; const unsigned int src_width = src_camera->width; const unsigned int src_height = src_camera->height; @@ -1634,6 +1637,11 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); if(debugg){ + +// cv::namedWindow( "src_det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_det_dilate", frame1->det_dilate); +// cv::namedWindow( "dst_det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "dst_det_dilate", frame2->det_dilate); +// cv::waitKey(0); + src_cloud->points.resize(src_width*src_height); for(unsigned int src_w = 0; src_w < src_width;src_w++){ for(unsigned int src_h = 0; src_h < src_height;src_h++){ @@ -1656,9 +1664,14 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d point.x = tx; point.y = ty; point.z = tz; - point.r = 255; - point.g = 0; - point.b = 0; + //point.r = 255*src_detdata[src_ind] != 0; +// point.g = 0; +// point.b = 0; + + point.r = src_detdata[src_ind]; + point.g = 0; + point.b = 0; + src_cloud->points[src_ind] = point; } } @@ -1683,11 +1696,17 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d point.z = dst_z; point.r = 0; point.g = 0; - point.b = 255; + point.b = dst_detdata[dst_ind]; dst_cloud->points[dst_ind] = point; } } } + + +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); +// viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); +// viewer->spin(); } int informations = 0; @@ -1697,7 +1716,7 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d int src_ind = src_h*src_width+src_w; float src_z = src_idepth*float(src_depthdata[src_ind]); float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ + if(src_z > 0 && src_nx != 2){ float src_ny = src_normalsdata[3*src_ind+1]; float src_nz = src_normalsdata[3*src_ind+2]; @@ -1717,7 +1736,7 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ + if(dst_z > 0 && dst_nx != 2){ float dst_ny = dst_normalsdata[3*dst_ind+1]; float dst_nz = dst_normalsdata[3*dst_ind+2]; @@ -1736,7 +1755,7 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d //d *= src_z;//compare_mul; double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + if(fabs(d) <= 0.0025){//If close, according noises, and angle of the surfaces similar: FUSE if(surface_angle > 0.8 && dst_maskdata[dst_ind] > 0){ if(interframe_connectionId.size() > 0){ double p_same = 0.9; @@ -1746,19 +1765,22 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d interframe_connectionId[src_ind+offset1].push_back(dst_ind+offset2); interframe_connectionStrength[src_ind+offset1].push_back(weight); } - overlaps[src_ind]++; - if(debugg){ - informations++; - dst_cloud->points[dst_ind].r = 0; - dst_cloud->points[dst_ind].g = 255; - dst_cloud->points[dst_ind].b = 0; - src_cloud->points[src_ind].r = 0; - src_cloud->points[src_ind].g = 255; - src_cloud->points[src_ind].b = 0; - } + if(dst_detdata[dst_ind] == 0 && src_detdata[src_ind] == 0){ + overlaps[src_ind]++; + if(debugg){ + informations++; + dst_cloud->points[dst_ind].r = 0; + dst_cloud->points[dst_ind].g = 255; + dst_cloud->points[dst_ind].b = 0; + src_cloud->points[src_ind].r = 0; + src_cloud->points[src_ind].g = 255; + src_cloud->points[src_ind].b = 0; + } + } } - }else if(tz < dst_z){ - occlusions[src_ind]++; + }else if(tz < dst_z && fabs(d) > 0.005 && dst_detdata[dst_ind] == 0 && src_detdata[src_ind] == 0){ + + occlusions[src_ind] ++; if(debugg){ informations++; src_cloud->points[src_ind].r = 255; @@ -1840,8 +1862,8 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int for(unsigned int w = 1; w < width-1;w++){ for(unsigned int h = 1; h < height-1;h++){ int ind = h*width+w; - maxima_dxdata[ind] = 255*(src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] >= src_dxdata[ind+1]); - maxima_dydata[ind] = 255*(src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] >= src_dydata[ind+width]); + maxima_dxdata[ind] = 255*(src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] > src_dxdata[ind+1]); + maxima_dydata[ind] = 255*(src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] > src_dydata[ind+width]); } } @@ -1874,8 +1896,8 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); func->zeromean = true; - func->maxp = 0.99; - func->startreg = 0.0; + func->maxp = 0.9999; + func->startreg = 0.5; func->debugg_print = false; func->bidir = true; func->maxd = 256.0; @@ -2017,13 +2039,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int float z2 = idepth*float(depthdata[other]); float dz = fabs(z-z2); - - //if(z3 > 0){z2 = 2*z2-z3;} if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} - - //dz = fabs(z-z2); - //dz = std::min(dz,fabs(z-z2)); - if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} } @@ -2037,25 +2053,8 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int float z2 = idepth*float(depthdata[other]); float dz = fabs(z-z2); - - //if(z3 > 0){z2 = 2*z2-z3;} - //if(z3 > 0){dz = std::min(dz,fabs(z- 2*z2-z3));} - - - //dz = std::min(dz,fabs(z-z2)); if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} - - // int other2 = ind+2*dir; - // int other = ind+dir; - - // float z3 = idepth*float(depthdata[other2]); - // float z2 = idepth*float(depthdata[other]); - // if(z3 > 0){z2 = 2*z2-z3;} - - // float dz = z-z2; - - // if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} } } } @@ -2064,8 +2063,89 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int probs.push_back(dx); probs.push_back(dy); } - //return probs; + + cv::Mat col_probs; + col_probs.create(height,width,CV_32FC3); + float * cpdata = (float *)col_probs.data; + + cv::Mat d_probs; + d_probs.create(height,width,CV_32FC3); + float * dpdata = (float *)d_probs.data; + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + + cpdata[3*ind+0] = 0; + cpdata[3*ind+1] = 0; + cpdata[3*ind+2] = 0; + + dpdata[3*ind+0] = 0; + dpdata[3*ind+1] = 0; + dpdata[3*ind+2] = 0; + + if(w > 0 && w < width-1){ + double ax = 0.5; + double bx = 0.5; + for(unsigned int p = 0; p < probs.size()-2; p+=2){ + double pr = probs[p][ind]; + ax *= pr; + bx *= 1.0-pr; + } + double px = ax/(ax+bx); + cpdata[3*ind+1] = (1-px) * float(maxima_dxdata[ind] > 0); + dpdata[3*ind+1] = (1-probs[probs.size()-2][ind]); + if(!frame->det_dilate.data[ind]){dpdata[3*ind+1] = (1-px) * float(maxima_dxdata[ind] > 0);} + //dpdata[3*ind+1] *= cpdata[3*ind+1]; + } + + if(h > 0 && h < height-1){ + + double ay = 0.5; + double by = 0.5; + for(unsigned int p = 1; p < probs.size()-2; p+=2){ + double pr = probs[p][ind]; + ay *= pr; + by *= 1.0-pr; + } + double py = ay/(ay+by); + cpdata[3*ind+2] = (1-py) * float(maxima_dydata[ind] > 0); + dpdata[3*ind+2] = (1-probs[probs.size()-1][ind]); + if(!frame->det_dilate.data[ind]){dpdata[3*ind+2] = (1-py) * float(maxima_dydata[ind] > 0);} + //dpdata[3*ind+2] *= cpdata[3*ind+2]; + } + } + } + + +// cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); +// cv::namedWindow( "col_probs", cv::WINDOW_AUTOSIZE ); cv::imshow( "col_probs", col_probs); +// cv::namedWindow( "d_probs", cv::WINDOW_AUTOSIZE ); cv::imshow( "d_probs", d_probs); +// // cv::namedWindow( "src_dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_dx", cv::abs(src_dx)); +// // cv::namedWindow( "src_dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_dy", cv::abs(src_dy)); +// // cv::namedWindow( "maxima_dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "maxima_dx",maxima_dx); +// // cv::namedWindow( "maxima_dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "maxima_dy",maxima_dy); + +// cv::waitKey(0); + + //return probs; + + std::vector dxc; + dxc.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){dxc[i] = 1-dpdata[3*i+1];} + + std::vector dyc; + dyc.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){dyc[i] = 1-dpdata[3*i+2];} + + + std::vector< std::vector > probs2; + probs2.push_back(dxc); + probs2.push_back(dyc); + return probs2; + +/* std::vector dxc; dxc.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0.5;} @@ -2130,6 +2210,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int // cv::waitKey(0); return probs2; + */ } std::vector doInference(std::vector & prior, std::vector< std::vector > & connectionId, std::vector< std::vector > & connectionStrength){ @@ -2686,7 +2767,13 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg){ - std::vector newmasks; +// +std::vector newmasks; +//for(double test = 0.99; test > 0.00000001; test *= 0.1) +{ +// double maxprob_same = 1-test;//0.99; + + double maxprob_same = 0.999999; double maxprob = 0.7; @@ -2726,7 +2813,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vectordepth.data); unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - + unsigned char * detdata = (unsigned char *)(frame->det_dilate.data); Camera * camera = frame->camera; const unsigned int width = camera->width; const unsigned int height = camera->height; @@ -2758,7 +2845,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector > probs = getImageProbs(frames[i],5); + std::vector< std::vector > probs = getImageProbs(frames[i],9); std::vector frame_prior; std::vector< std::vector > frame_connectionId; @@ -2779,7 +2866,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector= 1){ p_fg = maxprob;} else if(overlaps[j] >= 1){ p_fg = 0.4;} @@ -2790,11 +2877,40 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector 0 && w < width-1){ + int other = ind-1; + double p_same = std::min(probs[0][ind],maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + if(!std::isnan(weight) && weight > 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); + + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } + } + + if(h > 0 && h < height-1){ + int other = ind-width; + double p_same = std::min(probs[1][ind],maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + + if(!std::isnan(weight) && weight > 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); + + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } + } + +/* if(w > 0 && w < width-1){ double ax = 0.5; double bx = 0.5; @@ -2809,7 +2925,17 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector 0){ @@ -2836,8 +2962,17 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector ModelUpdater::computeDynamicObject(vector bgcp, vector ModelUpdater::computeDynamicObject(vector bgcp, vectorshow(false); - // cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); - // cv::imshow( "Original", originalmask ); - // cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); - // cv::imshow( "Improved", improvedmask ); - // cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); - // cv::imshow( "Difference", diffmask ); - // cv::waitKey(0); +// frame->show(false); +// //cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); +// //cv::imshow( "Original", originalmask ); +// cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "Improved", improvedmask ); +// //cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); +// //cv::imshow( "Difference", diffmask ); +// cv::waitKey(0); newmasks.push_back(improvedmask); } @@ -3082,7 +3218,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vectorremoveAllPointClouds(); } - +} return newmasks; } From 7c3d657f970871c1146b5d978a04757778a29d37 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 23 Aug 2016 17:12:14 +0200 Subject: [PATCH 073/255] improved segmentation app --- quasimodo/quasimodo_brain/CMakeLists.txt | 4 +- .../metaroom_additional_view_processing.cpp | 744 ++++++++++++------ quasimodo/quasimodo_models/CMakeLists.txt | 2 +- .../src/modelupdater/ModelUpdater.cpp | 2 +- semantic_map/src/semantic_map_recompute.cpp | 124 --- 5 files changed, 486 insertions(+), 390 deletions(-) diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 485ef362..a04d2766 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -109,9 +109,9 @@ target_link_libraries( preload_object_data ${catkin_LIBRARIES} ${PCL_LIBRARIES} add_dependencies( preload_object_data roscpp quasimodo_msgs_generate_messages_cpp) target_link_libraries( preload_object_data image_geometry cpp_common roscpp rosconsole tf_conversions ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) -add_executable( robot_listener src/robot_listener.cpp) +add_executable( robot_listener src/robot_listener.cpp) target_link_libraries( robot_listener ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${QT_LIBRARIES} metaroom_xml_parser) -add_dependencies( robot_listener roscpp quasimodo_msgs_generate_messages_cpp) +add_dependencies( robot_listener roscpp quasimodo_msgs_generate_messages_cpp) target_link_libraries( robot_listener image_geometry cpp_common roscpp rosconsole tf_conversions ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) add_executable( modelserver src/modelserver.cpp) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 03fddff0..1c565284 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -75,46 +75,13 @@ typedef semantic_map_load_utilties::DynamicObjectData ObjectData; using namespace std; using namespace semantic_map_load_utilties; -/* -bool segment_metaroom(quasimodo_msgs::metaroom_pair::Request & req, quasimodo_msgs::metaroom_pair::Response & res){ - printf("segment_metaroom\n"); - - printf("background: %s\n",req.background.c_str()); - printf("foreground: %s\n",req.foreground.c_str()); - - reglib::Model * bg_model = quasimodo_brain::load_metaroom_model(req.background); - reglib::Model * fg_model = quasimodo_brain::load_metaroom_model(req.foreground); - - quasimodo_msgs::segment_model sm; - sm.request.backgroundmodel = quasimodo_brain::getModelMSG(bg_model); - sm.request.models.push_back(quasimodo_brain::getModelMSG(fg_model)); - - bool status; - - if (segmentation_client.call(sm)){ - if(sm.response.dynamicmasks.size() > 0){ - res.dynamicmasks = sm.response.dynamicmasks.front().images; - res.movingmasks = sm.response.movingmasks.front().images; - } - status = true; - }else{ - ROS_ERROR("Failed to call service segment_model"); - status = false; - } - - bg_model->fullDelete(); - delete bg_model; - fg_model->fullDelete(); - delete fg_model; - return status; -} -*/ - std::string overall_folder = "~/.semanticMap/"; boost::shared_ptr viewer; int visualization_lvl = 0; std::string outtopic = "/some/topic"; ros::Publisher out_pub; +std::string posepath = ""; +std::vector sweepPoses; void remove_old_seg(std::string sweep_folder){ char buf [1024]; @@ -125,12 +92,288 @@ void remove_old_seg(std::string sweep_folder){ printf("%s\n",buf); } -void processMetaroom(std::string path){ - printf("processing: %s\n",path.c_str()); +void writePose(QXmlStreamWriter* xmlWriter, Eigen::Matrix4d pose){ + Eigen::Quaterniond q = Eigen::Quaterniond ((Eigen::Affine3d (pose)).rotation()); + + xmlWriter->writeStartElement("Translation"); + xmlWriter->writeStartElement("x"); + xmlWriter->writeCharacters(QString::number(pose(0,3))); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("y"); + xmlWriter->writeCharacters(QString::number(pose(1,3))); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("z"); + xmlWriter->writeCharacters(QString::number(pose(2,3))); + xmlWriter->writeEndElement(); + xmlWriter->writeEndElement(); // Translation + + xmlWriter->writeStartElement("Rotation"); + xmlWriter->writeStartElement("w"); + xmlWriter->writeCharacters(QString::number(q.w())); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("x"); + xmlWriter->writeCharacters(QString::number(q.x())); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("y"); + xmlWriter->writeCharacters(QString::number(q.y())); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("z"); + xmlWriter->writeCharacters(QString::number(q.z())); + xmlWriter->writeEndElement(); + xmlWriter->writeEndElement(); //Rotation +} + +void writeXml(std::string xmlFile, std::vector & frames, std::vector & poses){ + int slash_pos = xmlFile.find_last_of("/"); + std::string sweep_folder = xmlFile.substr(0, slash_pos) + "/"; + + QFile file(xmlFile.c_str()); + if (file.exists()){file.remove();} + + + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)) + { + std::cerr<<"Could not open file "<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Views"); + xmlWriter->writeAttribute("number_of_views", QString::number(frames.size())); + for(unsigned int i = 0; i < frames.size(); i++){ + reglib::RGBDFrame * frame = frames[i]; + char buf [1024]; + + xmlWriter->writeStartElement("View"); + sprintf(buf,"%s/view_RGB%10.10i.png",sweep_folder.c_str(),i); + cv::imwrite(buf, frame->rgb ); + xmlWriter->writeAttribute("RGB", QString(buf)); + + + sprintf(buf,"%s/view_DEPTH%10.10i.png",sweep_folder.c_str(),i); + cv::imwrite(buf, frame->depth ); + xmlWriter->writeAttribute("DEPTH", QString(buf)); + + long nsec = 1e9*((frame->capturetime)-std::floor(frame->capturetime)); + long sec = frame->capturetime; + xmlWriter->writeStartElement("Stamp"); + + xmlWriter->writeStartElement("sec"); + xmlWriter->writeCharacters(QString::number(sec)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("nsec"); + xmlWriter->writeCharacters(QString::number(nsec)); + xmlWriter->writeEndElement(); + + xmlWriter->writeEndElement(); // Stamp + + xmlWriter->writeStartElement("Camera"); + + xmlWriter->writeStartElement("fx"); + xmlWriter->writeCharacters(QString::number(frame->camera->fx)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("fy"); + xmlWriter->writeCharacters(QString::number(frame->camera->fy)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("cx"); + xmlWriter->writeCharacters(QString::number(frame->camera->cx)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("cy"); + xmlWriter->writeCharacters(QString::number(frame->camera->cy)); + xmlWriter->writeEndElement(); + + xmlWriter->writeEndElement(); // camera + + xmlWriter->writeStartElement("RegisteredPose"); + writePose(xmlWriter,poses[i]); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("Pose"); + writePose(xmlWriter,frame->pose); + xmlWriter->writeEndElement(); + + xmlWriter->writeEndElement(); + } + xmlWriter->writeEndElement(); // Semantic Room + xmlWriter->writeEndDocument(); + delete xmlWriter; +} + +Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader){ + QXmlStreamReader::TokenType token = xmlReader->readNext();//Translation + QString elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//fx + double tx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//fy + double ty = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cx + double tz = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Translation + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//Rotation + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//qw + double qw = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qx + double qx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qy + double qy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qz + double qz = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Rotation + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d regpose = (Eigen::Affine3d(Eigen::Quaterniond(qw,qx,qy,qz))).matrix(); + regpose(0,3) = tx; + regpose(0,3) = ty; + regpose(0,3) = tz; + + return regpose; +} + +void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses){ + + QFile file(xmlFile.c_str()); + + if (!file.exists()) + { + ROS_ERROR("Could not open file %s to load room.",xmlFile.c_str()); + return; + } + + QString xmlFileQS(xmlFile.c_str()); + int index = xmlFileQS.lastIndexOf('/'); + QString roomFolder = xmlFileQS.left(index); + + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()) + { + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()) + { + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + return; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement) + { + + if (xmlReader->name() == "View") + { + cv::Mat rgb; + cv::Mat depth; + //printf("elementName: %s\n",elementName.toStdString().c_str()); + QXmlStreamAttributes attributes = xmlReader->attributes(); + if (attributes.hasAttribute("RGB")) + { + QString rgbpath = attributes.value("RGB").toString(); + //printf("rgb filename: %s\n",rgbpath.toStdString().c_str()); + rgb = cv::imread(rgbpath.toStdString().c_str(), CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + if (attributes.hasAttribute("DEPTH")) + { + QString depthpath = attributes.value("DEPTH").toString(); + //printf("depth filename: %s\n",depthpath.toStdString().c_str()); + depth = cv::imread(depthpath.toStdString().c_str(), CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + token = xmlReader->readNext();//Stamp + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//sec + elementName = xmlReader->name().toString(); + int sec = atoi(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//nsec + elementName = xmlReader->name().toString(); + int nsec = atoi(xmlReader->readElementText().toStdString().c_str()); + token = xmlReader->readNext();//end stamp + + token = xmlReader->readNext();//Camera + elementName = xmlReader->name().toString(); + + reglib::Camera * cam = new reglib::Camera(); + + token = xmlReader->readNext();//fx + cam->fx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//fy + cam->fy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cx + cam->cx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cy + cam->cy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Camera + elementName = xmlReader->name().toString(); + + double time = double(sec)+double(nsec)/double(1e9); + + token = xmlReader->readNext();//RegisteredPose + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d regpose = getPose(xmlReader); + + token = xmlReader->readNext();//RegisteredPose + elementName = xmlReader->name().toString(); + + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d pose = getPose(xmlReader); + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, pose); + frames.push_back(frame); + poses.push_back(pose); + } + } + } + delete xmlReader; +} + +reglib::Model * processAV(std::string path){ + printf("processAV: %s\n",path.c_str()); int slash_pos = path.find_last_of("/"); std::string sweep_folder = path.substr(0, slash_pos) + "/"; -remove_old_seg(sweep_folder); + std::vector viewrgbs; std::vector viewdepths; std::vector viewtfs; @@ -161,18 +404,14 @@ remove_old_seg(sweep_folder); viewrgbs.push_back(rgb); viewdepths.push_back(depth); viewtfs.push_back(object.vAdditionalViewsTransforms[i]); - //viewcams.push_back(roomData.vIntermediateRoomCloudCamParams.front()); - - cv::namedWindow("rgbimage", cv::WINDOW_AUTOSIZE); - cv::imshow( "rgbimage", rgb); - cv::namedWindow("depthimage", cv::WINDOW_AUTOSIZE); - cv::imshow( "depthimage", depth); - cv::waitKey(30); } } reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path); - + for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ + sweep->frames[i].pose = sweep->frames.front().pose * sweepPoses[i]; + sweep->relativeposes[i] = sweepPoses[i]; + } std::vector frames; std::vector masks; @@ -206,6 +445,8 @@ remove_old_seg(sweep_folder); reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m);//a.matrix()); frames.push_back(frame); + + //both_unrefined.push_back(sweep->frames.front()->pose.inverse()*a.matrix()); both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); } @@ -216,7 +457,6 @@ remove_old_seg(sweep_folder); mu->massreg_timeout = 60*4; mu->viewer = viewer; - sweep->points = mu->getSuperPoints(sweep->relativeposes,sweep->frames,sweep->modelmasks,1,false); //Not needed if metaroom well calibrated @@ -234,29 +474,10 @@ remove_old_seg(sweep_folder); bgmassreg->setData(frames,masks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); - reglib::Model * av = new reglib::Model(); - av->frames = frames; - av->modelmasks = masks; - for(unsigned int i = 0; i < frames.size(); i++){ - av->relativeposes.push_back(bgmfr.poses[1].inverse()*bgmfr.poses[i+1]); - } - - - av->points = mu->getSuperPoints(av->relativeposes,av->frames,av->modelmasks,1,false); - for(unsigned int i = 0; i < frames.size(); i++){ frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; } - - // reglib::Model * fullmodel = new reglib::Model(); - // fullmodel->submodels.push_back(sweep); - // fullmodel->submodels_relativeposes.push_back(bgmfr.poses[0]); - // if(frames.size() > 0){ - // fullmodel->submodels.push_back(av); - // fullmodel->submodels_relativeposes.push_back(bgmfr.poses[1]); - // } - reglib::Model * fullmodel = new reglib::Model(); fullmodel->frames = sweep->frames; fullmodel->relativeposes = sweep->relativeposes; @@ -267,6 +488,99 @@ remove_old_seg(sweep_folder); fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); } + delete bgmassreg; + delete reg; + delete mu; + delete sweep; + + return fullmodel; +} + +void savePoses(std::string xmlFile, std::vector poses, int maxposes = -1){ + QFile file(xmlFile.c_str()); + if (file.exists()){file.remove();} + + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){ + std::cerr<<"Could not open file "<< xmlFile <<" to save views as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); +// xmlWriter->writeStartElement("Poses"); +// xmlWriter->writeAttribute("number_of_poses", QString::number(poses.size())); + for(unsigned int i = 0; i < poses.size() && (i == -1 || i < maxposes); i++){ + xmlWriter->writeStartElement("Pose"); + writePose(xmlWriter,poses[i]); + xmlWriter->writeEndElement(); + } +// xmlWriter->writeEndElement(); // Poses + xmlWriter->writeEndDocument(); + delete xmlWriter; +} + +std::vector readPoseXML(std::string xmlFile){ + std::vector poses; + QFile file(xmlFile.c_str()); + + if (!file.exists()){ + ROS_ERROR("Could not open file %s to load poses.",xmlFile.c_str()); + return poses; + } + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()){ + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()) + { + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + return poses; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "Pose"){ + Eigen::Matrix4d pose = getPose(xmlReader); + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + poses.push_back(pose); + } + } + } + delete xmlReader; + return poses; +} + +void processMetaroom(std::string path){ + printf("processing: %s\n",path.c_str()); + + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + reglib::Model * fullmodel = processAV(path); + + savePoses(sweep_folder+"testposes.xml",fullmodel->relativeposes,17); + + writeXml(path+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( fullmodel, reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + std::vector po; std::vector fr; std::vector mm; @@ -279,105 +593,16 @@ remove_old_seg(sweep_folder); if(overall_folder.back() == '/'){overall_folder.pop_back();} - int prevind = -1; std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); for (int i = 0; i < sweep_xmls.size(); i++){ - SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); std::string other_waypointid = other_roomData.roomWaypointId; - - if(sweep_xmls[i].compare(path) == 0){ - break; - } - if(other_waypointid.compare(current_waypointid) == 0){ - prevind = i; - } + if(sweep_xmls[i].compare(path) == 0){break;} + if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} } - - QFile file((sweep_folder+"ViewGroup.xml").c_str()); - if (file.exists()){file.remove();} - - - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){ - std::cerr<<"Could not open file "<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("ViewGroup"); - - for(unsigned int i = 0; i < fullmodel->frames.size(); i++){ - char buf [1024]; - xmlWriter->writeStartElement("View"); -// sprintf(buf,"%s/RGB%10.10i.png",sweep_folder.c_str(),i); -// cv::imwrite(buf, fullmodel->frames[i]->rgb ); -// sprintf(buf,"%s/DEPTH%10.10i.png",sweep_folder.c_str(),i); -// cv::imwrite(buf, fullmodel->frames[i]->depth ); - -// xmlWriter->writeStartElement("AdditionalViewTransform"); - -// geometry_msgs::TransformStamped msg; -// tf::transformStampedTFToMsg(viewtfs[i], msg); - -// xmlWriter->writeStartElement("Stamp"); -// xmlWriter->writeStartElement("sec"); -// xmlWriter->writeCharacters(QString::number(msg.header.stamp.sec)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeStartElement("nsec"); -// xmlWriter->writeCharacters(QString::number(msg.header.stamp.nsec)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeEndElement(); // Stamp - -// xmlWriter->writeStartElement("FrameId"); -// xmlWriter->writeCharacters(QString(msg.header.frame_id.c_str())); -// xmlWriter->writeEndElement(); - -// xmlWriter->writeStartElement("ChildFrameId"); -// xmlWriter->writeCharacters(QString(msg.child_frame_id.c_str())); -// xmlWriter->writeEndElement(); - -// xmlWriter->writeStartElement("Transform"); -// xmlWriter->writeStartElement("Translation"); -// xmlWriter->writeStartElement("x"); -// xmlWriter->writeCharacters(QString::number(msg.transform.translation.x)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeStartElement("y"); -// xmlWriter->writeCharacters(QString::number(msg.transform.translation.y)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeStartElement("z"); -// xmlWriter->writeCharacters(QString::number(msg.transform.translation.z)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeEndElement(); // Translation -// xmlWriter->writeStartElement("Rotation"); -// xmlWriter->writeStartElement("w"); -// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeStartElement("x"); -// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeStartElement("y"); -// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeStartElement("z"); -// xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z)); -// xmlWriter->writeEndElement(); -// xmlWriter->writeEndElement(); // Rotation -// xmlWriter->writeEndElement(); // Transform -// xmlWriter->writeEndElement(); // RoomIntermediateCloudTransform - xmlWriter->writeEndElement(); // View - } - xmlWriter->writeEndElement(); // ViewGroup - xmlWriter->writeEndDocument(); - delete xmlWriter; - -exit(0); - if(prevind != -1){ std::string prev = sweep_xmls[prevind]; printf("prev: %s\n",prev.c_str()); @@ -386,18 +611,14 @@ exit(0); bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); std::vector< reglib::Model * > models; - //models.push_back(sweep); models.push_back(fullmodel); - //models.push_back(av); - std::vector< std::vector< cv::Mat > > internal; std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; quasimodo_brain::segment(bg,models,internal,external,dynamic,true); - - + remove_old_seg(sweep_folder); for(unsigned int i = 0; i < models.size(); i++){ std::vector internal_masks = internal[i]; @@ -510,6 +731,19 @@ exit(0); cloud_cluster->points.push_back(dynamiccloud->points[pid]); } + char buf [1024]; + sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),d); + QFile file(buf); + if (file.exists()){file.remove();} + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Object"); + xmlWriter->writeAttribute("object_number", QString::number(d)); + for(unsigned int j = 0; j < mod_fr.size(); j++){ if(inds[j].size() == 0){continue;} reglib::RGBDFrame * frame = mod_fr[j]; @@ -523,17 +757,15 @@ exit(0); char buf [1024]; sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),d,j); cv::imwrite(buf, mask ); - - cv::namedWindow( "mask", cv::WINDOW_AUTOSIZE ); cv::imshow( "mask", mask); - cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb); - cv::waitKey(0); + xmlWriter->writeStartElement("Mask"); + xmlWriter->writeAttribute("filename", QString(buf)); + xmlWriter->writeAttribute("image_number", QString::number(j)); + xmlWriter->writeEndElement(); } - if(visualization_lvl > 0){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "scloud"); - viewer->spin(); - } + xmlWriter->writeEndElement(); + xmlWriter->writeEndDocument(); + delete xmlWriter; } // Creating the KdTree object for the search method of the extraction @@ -560,62 +792,57 @@ exit(0); cloud_cluster->points.push_back(movingcloud->points[pid]); } - if(visualization_lvl > 0){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "scloud"); - viewer->spin(); + char buf [1024]; + sprintf(buf,"%s/moving_object%10.10i.xml",sweep_folder.c_str(),d); + QFile file(buf); + if (file.exists()){file.remove();} + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save moving object as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Object"); + xmlWriter->writeAttribute("object_number", QString::number(d)); + + for(unsigned int j = 0; j < mod_fr.size(); j++){ + if(inds[j].size() == 0){continue;} + reglib::RGBDFrame * frame = mod_fr[j]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 0;} + for(unsigned int k = 0; k < inds[j].size(); k++){maskdata[inds[j][k]] = 255;} + + char buf [1024]; + sprintf(buf,"%s/movingmask_%i_%i.png",sweep_folder.c_str(),d,j); + cv::imwrite(buf, mask ); + xmlWriter->writeStartElement("Mask"); + xmlWriter->writeAttribute("filename", QString(buf)); + xmlWriter->writeAttribute("image_number", QString::number(j)); + xmlWriter->writeEndElement(); } + + xmlWriter->writeEndElement(); + xmlWriter->writeEndDocument(); + delete xmlWriter; } if(visualization_lvl > 0){ viewer->removeAllPointClouds(); viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); viewer->spin(); - - viewer->removeAllPointClouds(); - viewer->addPointCloud (dynamiccloud, pcl::visualization::PointCloudColorHandlerRGBField(dynamiccloud), "scloud"); - viewer->spin(); - - viewer->removeAllPointClouds(); - viewer->addPointCloud (movingcloud, pcl::visualization::PointCloudColorHandlerRGBField(movingcloud), "scloud"); - viewer->spin(); } - } - } - delete bgmassreg; +// delete bgmassreg; delete reg; delete mu; - // xmlWriter->writeStartElement("Transform"); - // xmlWriter->writeStartElement("Translation"); - // xmlWriter->writeStartElement("x"); - // xmlWriter->writeCharacters(QString::number(msg.transform.translation.x)); - // xmlWriter->writeEndElement(); - // xmlWriter->writeStartElement("y"); - // xmlWriter->writeCharacters(QString::number(msg.transform.translation.y)); - // xmlWriter->writeEndElement(); - // xmlWriter->writeStartElement("z"); - // xmlWriter->writeCharacters(QString::number(msg.transform.translation.z)); - // xmlWriter->writeEndElement(); - // xmlWriter->writeEndElement(); // Translation - // xmlWriter->writeStartElement("Rotation"); - // xmlWriter->writeStartElement("w"); - // xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w)); - // xmlWriter->writeEndElement(); - // xmlWriter->writeStartElement("x"); - // xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x)); - // xmlWriter->writeEndElement(); - // xmlWriter->writeStartElement("y"); - // xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y)); - // xmlWriter->writeEndElement(); - // xmlWriter->writeStartElement("z"); - // xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z)); - std_msgs::String msg; msg.data = path; out_pub.publish(msg); @@ -626,9 +853,37 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg){ processMetaroom(msg->data); } -void segmentWithAdditionalViewsXml(std::string sweep_xml,std::string prev){ - printf("segmentWithAdditionalViewsXml\n"); - // std::string prev = getPrevXML(sweep_xml); + +void trainMetaroom(std::string path){ + printf("processing: %s\n",path.c_str()); + if(posepath.compare("")==0){ + printf("posepath not set, set before training\n"); + return ; + } + + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + reglib::Model * fullmodel = processAV(path); + + //Not needed if metaroom well calibrated + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); + bgmassreg->timeout = 600; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = true; + bgmassreg->visualizationLvl = visualization_lvl; + bgmassreg->maskstep = 5; + bgmassreg->nomaskstep = 5; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); + + savePoses(path+"/testposes.xml",bgmfr.poses,17); + fullmodel->fullDelete(); + delete fullmodel; + delete bgmassreg; } int main(int argc, char** argv){ @@ -640,10 +895,13 @@ int main(int argc, char** argv){ int inputstate = 0; for(int i = 1; i < argc;i++){ printf("input: %s\n",argv[i]); - if(std::string(argv[i]).compare("-file") == 0){inputstate = 2;} - else if(std::string(argv[i]).compare("-intopic") == 0){inputstate = 0;} - else if(std::string(argv[i]).compare("-outtopic") == 0){inputstate = 1;} - else if(std::string(argv[i]).compare("-folder") == 0){inputstate = 4;} + if(std::string(argv[i]).compare("-file") == 0){ inputstate = 2;} + else if(std::string(argv[i]).compare("-intopic") == 0){ inputstate = 0;} + else if(std::string(argv[i]).compare("-outtopic") == 0){ inputstate = 1;} + else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 4;} + else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} + else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} + else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} else if(std::string(argv[i]).compare("-v") == 0){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0.5, 0, 0.5); @@ -661,54 +919,16 @@ int main(int argc, char** argv){ visualization_lvl = atoi(argv[i]); }else if(inputstate == 4){ overall_folder = std::string(argv[i]); + }else if(inputstate == 5){ + trainMetaroom(std::string(argv[i])); + }else if(inputstate == 6){ + posepath = std::string(argv[i]); + }else if(inputstate == 7){ + sweepPoses = readPoseXML(std::string(argv[i])); }else{ out_pub = n.advertise(outtopic, 1000); processMetaroom(std::string(argv[i])); } } - /* - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - - - - for(int ar = 1; ar < argc; ar++){ - string overall_folder = std::string(argv[ar]); - vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); - printf("sweep_xmls\n"); - for (auto sweep_xml : sweep_xmls) { - printf("sweep_xml: %s\n",sweep_xml.c_str()); - load2(sweep_xml); - } - } - - - - - ros::init(argc, argv, "test_segment"); - ros::NodeHandle n; - ros::ServiceClient segmentation_client = n.serviceClient("segment_model"); - - for(unsigned int i = 1; i < models.size(); i++){ - quasimodo_msgs::segment_model sm; - sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); - - if(i > 0){ - sm.request.backgroundmodel = quasimodo_brain::getModelMSG(models[i-1]); - } - - if (segmentation_client.call(sm)){//Build model from frame - //int model_id = mff.response.model_id; - printf("segmented: %i\n",i); - }else{ROS_ERROR("Failed to call service segment_model");} - } - //ros::spin(); - - delete reg; - for(size_t j = 0; j < models.size(); j++){ - models[j]->fullDelete(); - delete models[j]; - } - printf("done\n"); - */ return 0; } diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 9c8c56f6..0eee28df 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -56,7 +56,7 @@ add_library(quasimodo_weightlib src/weightfunctions/DistanceWeightFunction2.cpp target_link_libraries(quasimodo_weightlib ceres glog ${catkin_LIBRARIES}) add_library(quasimodo_core src/core/Util.cpp src/core/Camera.cpp src/core/RGBDFrame.cpp) -target_link_libraries(quasimodo_core ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries(quasimodo_core quasimodo_weightlib ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) add_library(quasimodo_reglib src/registration/Registration.cpp src/registration/RegistrationRandom.cpp src/registration/RegistrationRefinement.cpp) target_link_libraries(quasimodo_reglib quasimodo_weightlib ceres glog ${catkin_LIBRARIES}) diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 2a079e32..a8f6c4e8 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1758,7 +1758,7 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d if(fabs(d) <= 0.0025){//If close, according noises, and angle of the surfaces similar: FUSE if(surface_angle > 0.8 && dst_maskdata[dst_ind] > 0){ if(interframe_connectionId.size() > 0){ - double p_same = 0.9; + double p_same = 0.99; double not_p_same = 1-p_same; double weight = -log(not_p_same); diff --git a/semantic_map/src/semantic_map_recompute.cpp b/semantic_map/src/semantic_map_recompute.cpp index 24817bf2..37623ab9 100644 --- a/semantic_map/src/semantic_map_recompute.cpp +++ b/semantic_map/src/semantic_map_recompute.cpp @@ -212,131 +212,7 @@ std::string getPrevXML(std::string xml_file_name){ // } } -void fixXml(std::string sweep_xml){ - int slash_pos = sweep_xml.find_last_of("/"); - std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; - - std::vector viewrgbs; - std::vector viewdepths; - std::vector viewtfs; - - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); - for (auto objectFile : objectFiles){ - auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); - for (unsigned int i=0; iheight,cloud->width,CV_8UC3); - unsigned char * rgbdata = (unsigned char *)rgb.data; - - cv::Mat depth; - depth.create(cloud->height,cloud->width,CV_16UC1); - unsigned short * depthdata = (unsigned short *)depth.data; - - unsigned int nr_data = cloud->height * cloud->width; - for(unsigned int j = 0; j < nr_data; j++){ - PointType p = cloud->points[j]; - rgbdata[3*j+0] = p.b; - rgbdata[3*j+1] = p.g; - rgbdata[3*j+2] = p.r; - depthdata[j] = short(5000.0 * p.z); - } - - viewrgbs.push_back(rgb); - viewdepths.push_back(depth); - viewtfs.push_back(object.vAdditionalViewsTransforms[i]); - //viewcams.push_back(roomData.vIntermediateRoomCloudCamParams.front()); - - cv::namedWindow("rgbimage", cv::WINDOW_AUTOSIZE); - cv::imshow( "rgbimage", rgb); - cv::namedWindow("depthimage", cv::WINDOW_AUTOSIZE); - cv::imshow( "depthimage", depth); - cv::waitKey(30); - - } - } - - QFile file((sweep_folder+"additionalViews.xml").c_str()); - if (file.exists()){file.remove();} - - - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)) - { - std::cerr<<"Could not open file "<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("AdditionalViews"); - - for(unsigned int i = 0; i < viewrgbs.size(); i++){ - char buf [1024]; - sprintf(buf,"%s/RGB%10.10i.png",sweep_folder.c_str(),i); - cv::imwrite(buf, viewrgbs[i] ); - sprintf(buf,"%s/DEPTH%10.10i.png",sweep_folder.c_str(),i); - cv::imwrite(buf, viewdepths[i] ); - - xmlWriter->writeStartElement("AdditionalViewTransform"); - - geometry_msgs::TransformStamped msg; - tf::transformStampedTFToMsg(viewtfs[i], msg); - - xmlWriter->writeStartElement("Stamp"); - xmlWriter->writeStartElement("sec"); - xmlWriter->writeCharacters(QString::number(msg.header.stamp.sec)); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("nsec"); - xmlWriter->writeCharacters(QString::number(msg.header.stamp.nsec)); - xmlWriter->writeEndElement(); - xmlWriter->writeEndElement(); // Stamp - - xmlWriter->writeStartElement("FrameId"); - xmlWriter->writeCharacters(QString(msg.header.frame_id.c_str())); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("ChildFrameId"); - xmlWriter->writeCharacters(QString(msg.child_frame_id.c_str())); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("Transform"); - xmlWriter->writeStartElement("Translation"); - xmlWriter->writeStartElement("x"); - xmlWriter->writeCharacters(QString::number(msg.transform.translation.x)); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("y"); - xmlWriter->writeCharacters(QString::number(msg.transform.translation.y)); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("z"); - xmlWriter->writeCharacters(QString::number(msg.transform.translation.z)); - xmlWriter->writeEndElement(); - xmlWriter->writeEndElement(); // Translation - xmlWriter->writeStartElement("Rotation"); - xmlWriter->writeStartElement("w"); - xmlWriter->writeCharacters(QString::number(msg.transform.rotation.w)); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("x"); - xmlWriter->writeCharacters(QString::number(msg.transform.rotation.x)); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("y"); - xmlWriter->writeCharacters(QString::number(msg.transform.rotation.y)); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("z"); - xmlWriter->writeCharacters(QString::number(msg.transform.rotation.z)); - xmlWriter->writeEndElement(); - xmlWriter->writeEndElement(); // Rotation - xmlWriter->writeEndElement(); // Transform - xmlWriter->writeEndElement(); // RoomIntermediateCloudTransform - } - xmlWriter->writeEndElement(); // Semantic Room - xmlWriter->writeEndDocument(); - delete xmlWriter; -} std::string outtopic = "/some/topic"; ros::Publisher out_pub; From 718cd0941d9f43b13c6c107f97b580b36e1efee4 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 24 Aug 2016 17:17:17 +0200 Subject: [PATCH 074/255] imp seg --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 193 +---------- .../metaroom_additional_view_processing.cpp | 306 +++++++++--------- .../src/modelupdater/ModelUpdater.cpp | 40 ++- 3 files changed, 192 insertions(+), 347 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 5e751519..d36128ee 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -216,11 +216,11 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ boost::shared_ptr viewer; - //if(debugg){ - viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); - viewer->addCoordinateSystem(0.01); - viewer->setBackgroundColor(1.0,1.0,1.0); - //} + if(debugg){ + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); + viewer->addCoordinateSystem(0.01); + viewer->setBackgroundColor(1.0,1.0,1.0); + } reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); massregmod->timeout = 1200; @@ -238,30 +238,6 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec mu->massreg_timeout = 60*4; mu->viewer = viewer; - // std::vector bg_po; - // std::vector bg_fr; - // std::vector bg_mm; - // bg->getData(bg_po, bg_fr, bg_mm); - // bg->points = mu->getSuperPoints(bg_po,bg_fr,bg_mm,1,false); - - // std::vector< std::vector > mod_po_vec; - // std::vector< std::vector > mod_fr_vec; - // std::vector< std::vector > mod_mm_vec; - - // for(int j = 0; j < models.size(); j++){ - // reglib::Model * mod = models[j]; - // std::vector mod_po; - // std::vector mod_fr; - // std::vector mod_mm; - // mod->getData(mod_po, mod_fr, mod_mm); - // mod->points = mu->getSuperPoints(mod_po,mod_fr,mod_mm,1,false); - - // mod_po_vec.push_back(mod_po); - // mod_fr_vec.push_back(mod_fr); - // mod_mm_vec.push_back(mod_mm); - // printf("model[%i]->points = %i frames: %i\n",j,mod->points.size(),mod_fr.size()); - // } - if(models.size() > 0 && bg->frames.size() > 0){ std::vector cpmod; @@ -275,29 +251,13 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); cpmod.push_back(bg->frames.front()->pose.inverse() * models[j]->frames.front()->pose);//bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); massregmod->addModel(models[j]); - - // reglib::Model * mod = models[j]; - //// std::vector mod_po; - //// std::vector mod_fr; - //// std::vector mod_mm; - //// mod->getData(mod_po, mod_fr, mod_mm); - // cpmod.push_back(bg_po.front().inverse() * mod_po_vec[j].front()); - // massregmod->addModel(mod); - - } - printf("TIME TO REGISTER\n"); - reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); - printf("REGISTRATION DONE\n"); + reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); for(int j = 0; j < models.size(); j++){ Eigen::Matrix4d change = mfrmod.poses[j+1];// * cpmod[j+1].inverse(); - // for(unsigned int k = 0; k < mod_po_vec[j].size(); k++){ - // mod_po_vec[j][k] = change*mod_po_vec[j][k]; - // } - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; } @@ -315,25 +275,12 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); massregmod->addModel(models[j]); - - // reglib::Model * mod = models[j]; - //// std::vector mod_po; - //// std::vector mod_fr; - //// std::vector mod_mm; - //// mod->getData(mod_po, mod_fr, mod_mm); - //// mod->points = mu->getSuperPoints(mod_po,mod_fr,mod_mm,1,false); - //// if(j == 0){first = mod_po.front();} - // cpmod.push_back(first.inverse() * mod_po_vec[j].front()); - // massregmod->addModel(mod); } reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); for(int j = 0; j < models.size(); j++){ Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); - // for(unsigned int k = 0; k < mod_po_vec[j].size(); k++){ - // mod_po_vec[j][k] = change*mod_po_vec[j][k]; - // } for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; } @@ -346,16 +293,7 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec delete massregmod; - - // std::vector bgcp = bg_po; - // std::vector bgcf = bg_fr; - // std::vector bgmask; - - // for(unsigned int k = 0; k < bg_mm.size(); k++){ - // bgmask.push_back(bg_mm[k]->getMask()); - // } - - std::vector bgcp; + std::vector bgcp; std::vector bgcf; std::vector bgmask; for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ @@ -368,8 +306,6 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec reglib::Model * model = models[j]; std::vector masks; - // for(unsigned int i = 0; i < mod_fr_vec[j].size(); i++){ - // reglib::RGBDFrame * frame = mod_fr_vec[j][i]; for(unsigned int i = 0; i < model->frames.size(); i++){ reglib::RGBDFrame * frame = model->frames[i]; reglib::Camera * cam = frame->camera; @@ -379,26 +315,13 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} masks.push_back(mask); } - // for(unsigned int i = 0; i < model->frames.size(); i++){ - // reglib::RGBDFrame * frame = model->frames[i]; - // reglib::Camera * cam = frame->camera; - // cv::Mat mask; - // mask.create(cam->height,cam->width,CV_8UC1); - // unsigned char * maskdata = (unsigned char *)(mask.data); - // for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} - // masks.push_back(mask); - // } //mu->computeMovingDynamicStatic(bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions // mu->computeMovingDynamicStatic(bgcp,bgcf,mod_po_vec[j],mod_fr_vec[j],debugg);//Determine self occlusions - //exit(0); - // std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,mod_po_vec[j],mod_fr_vec[j],masks,mod_po_vec[j],mod_fr_vec[j],masks,true);//Determine self occlusions - // std::vector external_masks = mu->computeDynamicObject(mod_po_vec[j],mod_fr_vec[j],masks,bgcp,bgcf,bgmask,mod_po_vec[j],mod_fr_vec[j],masks,true);//Determine external occlusions - std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,false);//Determine self occlusions - std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,false);//Determine external occlusions + + std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,debugg);//Determine self occlusions + std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,debugg);//Determine external occlusions std::vector dynamic_masks; - // for(unsigned int i = 0; i < mod_fr_vec[j].size(); i++){ - // reglib::RGBDFrame * frame = mod_fr_vec[j][i]; for(unsigned int i = 0; i < model->frames.size(); i++){ reglib::RGBDFrame * frame = model->frames[i]; reglib::Camera * cam = frame->camera; @@ -431,102 +354,6 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec dynamic.push_back(dynamic_masks); } - /* - for(unsigned int i = 0; visualization && i < models.size(); i++){ - std::vector internal_masks = internal[i]; - std::vector external_masks = external[i]; - std::vector dynamic_masks = dynamic[i]; - reglib::Model * model = models[i]; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - for(unsigned int j = 0; j < model->frames.size(); j++){ - reglib::RGBDFrame * frame = model->frames[j]; - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); - - reglib::Camera * camera = frame->camera; - - cv::imshow( "rgb", frame->rgb ); - cv::imshow( "internal_masks", internal_masks[j] ); - cv::imshow( "externalmask", external_masks[j] ); - cv::imshow( "dynamic_mask", dynamic_masks[j] ); - cv::waitKey(100); - - - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); - unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); - unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); - - Eigen::Matrix4d p = model->relativeposes[j]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - pcl::PointXYZRGBNormal point; - point.x = m00*x + m01*y + m02*z + m03; - point.y = m10*x + m11*y + m12*z + m13; - point.z = m20*x + m21*y + m22*z + m23; - - point.b = rgbdata[3*ind+0]; - point.g = rgbdata[3*ind+1]; - point.r = rgbdata[3*ind+2]; - - if(dynamicmaskdata[ind] != 0){ - point.b = 0; - point.g = 255; - point.r = 0; - }else if(internalmaskdata[ind] == 0){ - point.b = 0; - point.g = 0; - point.r = 255; - } - - cloud->points.push_back(point); - } - } - } - } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - while(cv::waitKey(50)!='q'){viewer->spinOnce();} - } - - res.backgroundmodel = req.backgroundmodel; - - - res.masks.resize(models.size()); - for(unsigned int i = 0; i < models.size(); i++){ - for(unsigned int j = 0; j < models[i]->frames.size(); j++){ - cv_bridge::CvImage maskBridgeImage; - maskBridgeImage.image = dynamic[i][j]; - maskBridgeImage.encoding = "mono8"; - res.masks[i].images.push_back( *(maskBridgeImage.toImageMsg()) ); - } - - res.models.push_back(quasimodo_brain::getModelMSG(models[i])); - models[i]->fullDelete(); - delete models[i]; - } - models.clear(); - */ - delete reg; delete mu; } diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 1c565284..60148a29 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -161,42 +161,42 @@ void writeXml(std::string xmlFile, std::vector & frames, st long sec = frame->capturetime; xmlWriter->writeStartElement("Stamp"); - xmlWriter->writeStartElement("sec"); - xmlWriter->writeCharacters(QString::number(sec)); - xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("sec"); + xmlWriter->writeCharacters(QString::number(sec)); + xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("nsec"); - xmlWriter->writeCharacters(QString::number(nsec)); - xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("nsec"); + xmlWriter->writeCharacters(QString::number(nsec)); + xmlWriter->writeEndElement(); xmlWriter->writeEndElement(); // Stamp xmlWriter->writeStartElement("Camera"); - xmlWriter->writeStartElement("fx"); - xmlWriter->writeCharacters(QString::number(frame->camera->fx)); - xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("fx"); + xmlWriter->writeCharacters(QString::number(frame->camera->fx)); + xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("fy"); - xmlWriter->writeCharacters(QString::number(frame->camera->fy)); - xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("fy"); + xmlWriter->writeCharacters(QString::number(frame->camera->fy)); + xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("cx"); - xmlWriter->writeCharacters(QString::number(frame->camera->cx)); - xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("cx"); + xmlWriter->writeCharacters(QString::number(frame->camera->cx)); + xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("cy"); - xmlWriter->writeCharacters(QString::number(frame->camera->cy)); - xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("cy"); + xmlWriter->writeCharacters(QString::number(frame->camera->cy)); + xmlWriter->writeEndElement(); xmlWriter->writeEndElement(); // camera xmlWriter->writeStartElement("RegisteredPose"); - writePose(xmlWriter,poses[i]); + writePose(xmlWriter,poses[i]); xmlWriter->writeEndElement(); xmlWriter->writeStartElement("Pose"); - writePose(xmlWriter,frame->pose); + writePose(xmlWriter,frame->pose); xmlWriter->writeEndElement(); xmlWriter->writeEndElement(); @@ -242,8 +242,8 @@ Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader){ Eigen::Matrix4d regpose = (Eigen::Affine3d(Eigen::Quaterniond(qw,qx,qy,qz))).matrix(); regpose(0,3) = tx; - regpose(0,3) = ty; - regpose(0,3) = tz; + regpose(1,3) = ty; + regpose(2,3) = tz; return regpose; } @@ -408,10 +408,10 @@ reglib::Model * processAV(std::string path){ } reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path); - for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ - sweep->frames[i].pose = sweep->frames.front().pose * sweepPoses[i]; - sweep->relativeposes[i] = sweepPoses[i]; - } +// for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ +// sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; +// sweep->relativeposes[i] = sweepPoses[i]; +// } std::vector frames; std::vector masks; @@ -445,8 +445,6 @@ reglib::Model * processAV(std::string path){ reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m);//a.matrix()); frames.push_back(frame); - - //both_unrefined.push_back(sweep->frames.front()->pose.inverse()*a.matrix()); both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); } @@ -465,9 +463,9 @@ reglib::Model * processAV(std::string path){ bgmassreg->viewer = viewer; bgmassreg->use_surface = true; bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = 0;//visualization_lvl; - bgmassreg->maskstep = 15; - bgmassreg->nomaskstep = 15; + bgmassreg->visualizationLvl = 0; + bgmassreg->maskstep = 5; + bgmassreg->nomaskstep = 5; bgmassreg->nomask = true; bgmassreg->stopval = 0.0005; bgmassreg->addModel(sweep); @@ -509,14 +507,15 @@ void savePoses(std::string xmlFile, std::vector poses, int maxp xmlWriter->setDevice(&file); xmlWriter->writeStartDocument(); -// xmlWriter->writeStartElement("Poses"); -// xmlWriter->writeAttribute("number_of_poses", QString::number(poses.size())); + xmlWriter->writeStartElement("Poses"); + // xmlWriter->writeAttribute("number_of_poses", QString::number(poses.size())); for(unsigned int i = 0; i < poses.size() && (i == -1 || i < maxposes); i++){ + printf("saving %i\n",i); xmlWriter->writeStartElement("Pose"); - writePose(xmlWriter,poses[i]); + writePose(xmlWriter,poses[i]); xmlWriter->writeEndElement(); } -// xmlWriter->writeEndElement(); // Poses + xmlWriter->writeEndElement(); // Poses xmlWriter->writeEndDocument(); delete xmlWriter; } @@ -555,6 +554,7 @@ std::vector readPoseXML(std::string xmlFile){ token = xmlReader->readNext();//Pose elementName = xmlReader->name().toString(); + std::cout << pose << std::endl << std::endl; poses.push_back(pose); } } @@ -707,139 +707,146 @@ void processMetaroom(std::string path){ } } - // Creating the KdTree object for the search method of the extraction - pcl::search::KdTree::Ptr dynamictree (new pcl::search::KdTree); - dynamictree->setInputCloud (dynamiccloud); - - std::vector dynamic_indices; - pcl::EuclideanClusterExtraction dynamic_ec; - dynamic_ec.setClusterTolerance (0.02); // 2cm - dynamic_ec.setMinClusterSize (500); - dynamic_ec.setMaxClusterSize (250000000); - dynamic_ec.setSearchMethod (dynamictree); - dynamic_ec.setInputCloud (dynamiccloud); - dynamic_ec.extract (dynamic_indices); - - for (unsigned int d = 0; d < dynamic_indices.size(); d++){ - std::vector< std::vector > inds; - inds.resize(mod_fr.size()); - - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ - int pid = dynamic_indices[d].indices[ind]; - inds[dynamic_frameid[pid]].push_back(dynamic_pixelid[pid]); - cloud_cluster->points.push_back(dynamiccloud->points[pid]); - } - - char buf [1024]; - sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),d); - QFile file(buf); - if (file.exists()){file.remove();} - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("Object"); - xmlWriter->writeAttribute("object_number", QString::number(d)); - - for(unsigned int j = 0; j < mod_fr.size(); j++){ - if(inds[j].size() == 0){continue;} - reglib::RGBDFrame * frame = mod_fr[j]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 0;} - for(unsigned int k = 0; k < inds[j].size(); k++){maskdata[inds[j][k]] = 255;} + printf("dynamiccloud: %i\n",dynamiccloud->points.size()); + if(dynamiccloud->points.size() > 0){ + // Creating the KdTree object for the search method of the extraction + pcl::search::KdTree::Ptr dynamictree (new pcl::search::KdTree); + dynamictree->setInputCloud (dynamiccloud); + + std::vector dynamic_indices; + pcl::EuclideanClusterExtraction dynamic_ec; + dynamic_ec.setClusterTolerance (0.02); // 2cm + dynamic_ec.setMinClusterSize (500); + dynamic_ec.setMaxClusterSize (250000000); + dynamic_ec.setSearchMethod (dynamictree); + dynamic_ec.setInputCloud (dynamiccloud); + dynamic_ec.extract (dynamic_indices); + + for (unsigned int d = 0; d < dynamic_indices.size(); d++){ + std::vector< std::vector > inds; + inds.resize(mod_fr.size()); + + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ + int pid = dynamic_indices[d].indices[ind]; + inds[dynamic_frameid[pid]].push_back(dynamic_pixelid[pid]); + cloud_cluster->points.push_back(dynamiccloud->points[pid]); + } char buf [1024]; - sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),d,j); - cv::imwrite(buf, mask ); - xmlWriter->writeStartElement("Mask"); - xmlWriter->writeAttribute("filename", QString(buf)); - xmlWriter->writeAttribute("image_number", QString::number(j)); + sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),d); + QFile file(buf); + if (file.exists()){file.remove();} + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Object"); + xmlWriter->writeAttribute("object_number", QString::number(d)); + + for(unsigned int j = 0; j < mod_fr.size(); j++){ + if(inds[j].size() == 0){continue;} + reglib::RGBDFrame * frame = mod_fr[j]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 0;} + for(unsigned int k = 0; k < inds[j].size(); k++){maskdata[inds[j][k]] = 255;} + + char buf [1024]; + sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),d,j); + cv::imwrite(buf, mask ); + xmlWriter->writeStartElement("Mask"); + xmlWriter->writeAttribute("filename", QString(buf)); + xmlWriter->writeAttribute("image_number", QString::number(j)); + xmlWriter->writeEndElement(); + } + xmlWriter->writeEndElement(); + xmlWriter->writeEndDocument(); + delete xmlWriter; } - - xmlWriter->writeEndElement(); - xmlWriter->writeEndDocument(); - delete xmlWriter; } - // Creating the KdTree object for the search method of the extraction - pcl::search::KdTree::Ptr movingtree (new pcl::search::KdTree); - movingtree->setInputCloud (movingcloud); - - std::vector moving_indices; - pcl::EuclideanClusterExtraction moving_ec; - moving_ec.setClusterTolerance (0.02); // 2cm - moving_ec.setMinClusterSize (500); - moving_ec.setMaxClusterSize (250000000); - moving_ec.setSearchMethod (movingtree); - moving_ec.setInputCloud (movingcloud); - moving_ec.extract (moving_indices); - - for (unsigned int d = 0; d < moving_indices.size(); d++){ - std::vector< std::vector > inds; - inds.resize(mod_fr.size()); - - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ - int pid = moving_indices[d].indices[ind]; - inds[moving_frameid[pid]].push_back(moving_pixelid[pid]); - cloud_cluster->points.push_back(movingcloud->points[pid]); - } - char buf [1024]; - sprintf(buf,"%s/moving_object%10.10i.xml",sweep_folder.c_str(),d); - QFile file(buf); - if (file.exists()){file.remove();} - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save moving object as XML"<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("Object"); - xmlWriter->writeAttribute("object_number", QString::number(d)); - - for(unsigned int j = 0; j < mod_fr.size(); j++){ - if(inds[j].size() == 0){continue;} - reglib::RGBDFrame * frame = mod_fr[j]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 0;} - for(unsigned int k = 0; k < inds[j].size(); k++){maskdata[inds[j][k]] = 255;} + printf("movingcloud: %i\n",movingcloud->points.size()); + if(movingcloud->points.size() > 0){ + // Creating the KdTree object for the search method of the extraction + pcl::search::KdTree::Ptr movingtree (new pcl::search::KdTree); + movingtree->setInputCloud (movingcloud); + + std::vector moving_indices; + pcl::EuclideanClusterExtraction moving_ec; + moving_ec.setClusterTolerance (0.02); // 2cm + moving_ec.setMinClusterSize (500); + moving_ec.setMaxClusterSize (250000000); + moving_ec.setSearchMethod (movingtree); + moving_ec.setInputCloud (movingcloud); + moving_ec.extract (moving_indices); + + for (unsigned int d = 0; d < moving_indices.size(); d++){ + std::vector< std::vector > inds; + inds.resize(mod_fr.size()); + + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ + int pid = moving_indices[d].indices[ind]; + inds[moving_frameid[pid]].push_back(moving_pixelid[pid]); + cloud_cluster->points.push_back(movingcloud->points[pid]); + } char buf [1024]; - sprintf(buf,"%s/movingmask_%i_%i.png",sweep_folder.c_str(),d,j); - cv::imwrite(buf, mask ); - xmlWriter->writeStartElement("Mask"); - xmlWriter->writeAttribute("filename", QString(buf)); - xmlWriter->writeAttribute("image_number", QString::number(j)); + sprintf(buf,"%s/moving_object%10.10i.xml",sweep_folder.c_str(),d); + QFile file(buf); + if (file.exists()){file.remove();} + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save moving object as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Object"); + xmlWriter->writeAttribute("object_number", QString::number(d)); + + for(unsigned int j = 0; j < mod_fr.size(); j++){ + if(inds[j].size() == 0){continue;} + reglib::RGBDFrame * frame = mod_fr[j]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 0;} + for(unsigned int k = 0; k < inds[j].size(); k++){maskdata[inds[j][k]] = 255;} + + char buf [1024]; + sprintf(buf,"%s/movingmask_%i_%i.png",sweep_folder.c_str(),d,j); + cv::imwrite(buf, mask ); + xmlWriter->writeStartElement("Mask"); + xmlWriter->writeAttribute("filename", QString(buf)); + xmlWriter->writeAttribute("image_number", QString::number(j)); + xmlWriter->writeEndElement(); + } + xmlWriter->writeEndElement(); + xmlWriter->writeEndDocument(); + delete xmlWriter; } - - xmlWriter->writeEndElement(); - xmlWriter->writeEndDocument(); - delete xmlWriter; } - if(visualization_lvl > 0){ viewer->removeAllPointClouds(); viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); viewer->spin(); } } + } -// delete bgmassreg; + // delete bgmassreg; delete reg; delete mu; @@ -872,7 +879,7 @@ void trainMetaroom(std::string path){ bgmassreg->viewer = viewer; bgmassreg->use_surface = true; bgmassreg->use_depthedge = true; - bgmassreg->visualizationLvl = visualization_lvl; + bgmassreg->visualizationLvl = 0; bgmassreg->maskstep = 5; bgmassreg->nomaskstep = 5; bgmassreg->nomask = true; @@ -880,7 +887,7 @@ void trainMetaroom(std::string path){ bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); - savePoses(path+"/testposes.xml",bgmfr.poses,17); + savePoses(posepath,bgmfr.poses,17); fullmodel->fullDelete(); delete fullmodel; delete bgmassreg; @@ -901,7 +908,7 @@ int main(int argc, char** argv){ else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 4;} else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} - else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} + else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} else if(std::string(argv[i]).compare("-v") == 0){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0.5, 0, 0.5); @@ -925,6 +932,7 @@ int main(int argc, char** argv){ posepath = std::string(argv[i]); }else if(inputstate == 7){ sweepPoses = readPoseXML(std::string(argv[i])); + //exit(0); }else{ out_pub = n.advertise(outtopic, 1000); processMetaroom(std::string(argv[i])); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index a8f6c4e8..3701baf5 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1723,10 +1723,11 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d float src_x = (float(src_w) - src_cx) * src_z * src_ifx; float src_y = (float(src_h) - src_cy) * src_z * src_ify; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + if(tz < 0){continue;} float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; float itz = 1.0/tz; float dst_w = dst_fx*tx*itz + dst_cx; float dst_h = dst_fy*ty*itz + dst_cy; @@ -1755,7 +1756,7 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d //d *= src_z;//compare_mul; double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - if(fabs(d) <= 0.0025){//If close, according noises, and angle of the surfaces similar: FUSE + if(fabs(d) <= 0.01){//If close, according noises, and angle of the surfaces similar: FUSE if(surface_angle > 0.8 && dst_maskdata[dst_ind] > 0){ if(interframe_connectionId.size() > 0){ double p_same = 0.99; @@ -1778,7 +1779,7 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d } } } - }else if(tz < dst_z && fabs(d) > 0.005 && dst_detdata[dst_ind] == 0 && src_detdata[src_ind] == 0){ + }else if(tz < dst_z && fabs(d) > 0.02 && dst_detdata[dst_ind] == 0 && src_detdata[src_ind] == 0){ occlusions[src_ind] ++; if(debugg){ @@ -2086,33 +2087,42 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int dpdata[3*ind+2] = 0; if(w > 0 && w < width-1){ - double ax = 0.5; - double bx = 0.5; + float ax = 0.5; + float bx = 0.5; for(unsigned int p = 0; p < probs.size()-2; p+=2){ - double pr = probs[p][ind]; + float pr = probs[p][ind]; ax *= pr; bx *= 1.0-pr; } - double px = ax/(ax+bx); + float px = ax/(ax+bx); cpdata[3*ind+1] = (1-px) * float(maxima_dxdata[ind] > 0); dpdata[3*ind+1] = (1-probs[probs.size()-2][ind]); if(!frame->det_dilate.data[ind]){dpdata[3*ind+1] = (1-px) * float(maxima_dxdata[ind] > 0);} + else{ + dpdata[3*ind+1] = std::max(dpdata[3*ind+1],0.8f*(1-px) * float(maxima_dxdata[ind] > 0)); + } //dpdata[3*ind+1] *= cpdata[3*ind+1]; } if(h > 0 && h < height-1){ - double ay = 0.5; - double by = 0.5; + float ay = 0.5; + float by = 0.5; for(unsigned int p = 1; p < probs.size()-2; p+=2){ - double pr = probs[p][ind]; + float pr = probs[p][ind]; ay *= pr; by *= 1.0-pr; } - double py = ay/(ay+by); + float py = ay/(ay+by); cpdata[3*ind+2] = (1-py) * float(maxima_dydata[ind] > 0); dpdata[3*ind+2] = (1-probs[probs.size()-1][ind]); if(!frame->det_dilate.data[ind]){dpdata[3*ind+2] = (1-py) * float(maxima_dydata[ind] > 0);} + else{ + dpdata[3*ind+2] = std::max(dpdata[3*ind+2],0.8f*(1-py) * float(maxima_dydata[ind] > 0)); + } + double maxw = std::max(dpdata[3*ind+1],dpdata[3*ind+2]); + dpdata[3*ind+1] = maxw; + dpdata[3*ind+2] = maxw; //dpdata[3*ind+2] *= cpdata[3*ind+2]; } } @@ -2833,7 +2843,7 @@ std::vector newmasks; for(unsigned int j = 0; j < bgcp.size(); j++){ if(frame == bgcf[j]){continue;} Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,debugg); + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); } for(unsigned int j = 0; j < nr_pixels; j++){ @@ -2843,7 +2853,7 @@ std::vector newmasks; for(unsigned int j = 0; j < cp.size(); j++){ if(frame == cf[j]){continue;} Eigen::Matrix4d p = poses[i].inverse() * cp[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, cf[j],mm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,debugg); + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, cf[j],mm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); } std::vector< std::vector > probs = getImageProbs(frames[i],9); @@ -3212,7 +3222,7 @@ std::vector newmasks; */ viewer->removeAllPointClouds(); viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "scloud"); - viewer->addPointCloud (bgcloud, pcl::visualization::PointCloudColorHandlerRGBField(bgcloud), "bgcloud"); + //viewer->addPointCloud (bgcloud, pcl::visualization::PointCloudColorHandlerRGBField(bgcloud), "bgcloud"); viewer->spin(); viewer->removeAllPointClouds(); From 2f54ad409de048c50f4530f502b7af3e2ffdbb01 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Thu, 25 Aug 2016 16:43:09 +0200 Subject: [PATCH 075/255] Incremental query now working also with mongodb queries --- .../dynamic_retrieval.h | 194 +++++++++++++++++- .../impl/grouped_vocabulary_tree.hpp | 18 +- .../k_means_tree/impl/vocabulary_tree.hpp | 1 + 3 files changed, 209 insertions(+), 4 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index fd4d2fd1..ea9ad3e1 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -52,6 +52,8 @@ enum uri_kind { all }; + + // this should have some switch for what to keep // if kind = uri_kind::mongodb, every index will be returned but we will only keep the first (what number) which are valid // alternatively, maybe return empty paths for everything else? sounds good, or rather, let's still return the files @@ -246,6 +248,167 @@ get_retrieved_path_scores(const std::vector::r { // OK, now we need to get the paths for all of the stuff within the scans instead, so basically // we need to get a subsegment_sweep__path_iterator and add all of the path corresponding to groups + + if (verbose) { + std::cout << "Entering get_retrieved_path_scores" << std::endl; + } + +#if WITH_UNIVERSAL_URIS + + using IndexT = grouped_vocabulary_tree::result_type; + + std::vector, grouped_vocabulary_tree::result_type> > path_scores; + + ros::NodeHandle n("/dynamic_retrieval"); + size_t offset = summary.nbr_noise_segments; + segment_uris uris; + std::cout << "Loading URI:s file: " << (boost::filesystem::path(summary.noise_data_path) / "vocabulary" / "segment_uris.json").string() << std::endl; + uris.load(boost::filesystem::path(summary.noise_data_path) / "vocabulary"); + mongodb_store::MessageStoreProxy* message_store = NULL; + + for (IndexT s : scores) { + std::cout << "1. Index: " << s.subgroup_global_indices[0] << std::endl; + } + + for (IndexT s : scores) { + int index = s.subgroup_global_indices[0]; + if (index >= offset) { + std::cout << "1. Index: " << index << " is larger than: " << offset << std::endl; + std::cout << "URI loading does not support noise+annotated data structure!" << std::endl; + std::cout << "Got URI: " << uris.uris[index] << std::endl; + exit(-1); + } + std::string uri = uris.uris[index]; + if (verbose) { + std::cout << "Got URI: " << uri << " at index: " << index << std::endl; + } + if (uri.compare(0, 10, "mongodb://") == 0) { + std::string db_uri = uri.substr(10, uri.size()-10); + std::vector strs; + boost::split(strs, db_uri, boost::is_any_of("/")); + std::cout << "Initializing message story proxy with database: " << strs[0] << ", collection: " << strs[1] << std::endl; + message_store = new mongodb_store::MessageStoreProxy(n, strs[1], strs[0]); + break; + } + } + + int temp = 0; + size_t loaded_mongodb_results = 0; + for (const grouped_vocabulary_tree::result_type& score : scores) { + int index = score.subgroup_global_indices[0]; + + std::cout << "Iteration nbr: " << temp << std::endl; + std::cout << "Scores size: " << scores.size() << std::endl; + std::cout << "Subsegments in group: " << score.subgroup_group_indices.size() << std::endl; + ++temp; + path_scores.push_back(std::make_pair(std::vector(), score)); + + // find the uri of the first segment to determine if mongodb result or metaroom result + std::string uri = uris.uris[index]; + if (uri.compare(0, 7, "file://") == 0) { + //retrieved_paths.push_back(boost::filesystem::path(uri.substr(7, uri.size()-7))); + + int sweep_id = score.group_index; + std::cout << "Getting sweep xml for group: " << sweep_id << std::endl; + std::cout << "With subsegment: " << score.subgroup_index << std::endl; + boost::filesystem::path sweep_path = dynamic_object_retrieval::get_sweep_xml(sweep_id, summary); + std::cout << "Getting a subsegment iterator for sweep: " << sweep_path.string() << std::endl; + if (summary.subsegment_type == "subsegment" || summary.subsegment_type == "supervoxel") { + dynamic_object_retrieval::sweep_subsegment_keypoint_map subsegments(sweep_path); // this is a problem + std::cout << "Finished getting a subsegment iterator for sweep: " << sweep_path.string() << std::endl; + int counter = 0; + for (const boost::filesystem::path& path : subsegments) { + if (std::find(score.subgroup_group_indices.begin(), score.subgroup_group_indices.end(), counter) != score.subgroup_group_indices.end()) { + path_scores.back().first.push_back(path); + } + ++counter; + } + } + else if (summary.subsegment_type == "convex_segment") { + dynamic_object_retrieval::sweep_convex_segment_map subsegments(sweep_path); // this is a problem + std::cout << "Finished getting a subsegment iterator for sweep: " << sweep_path.string() << std::endl; + int counter = 0; + for (const boost::filesystem::path& path : subsegments) { + if (std::find(score.subgroup_group_indices.begin(), score.subgroup_group_indices.end(), counter) != score.subgroup_group_indices.end()) { + path_scores.back().first.push_back(path); + } + ++counter; + } + } + else { + std::cout << summary.subsegment_type << " not a valid subsegment type..." << std::endl; + exit(-1); + } + + std::cout << "Finished getting sweep xml for group: " << sweep_id << std::endl; + std::cout << "Got " << path_scores.back().first.size() << " subsegments..." << std::endl; + } + else if (uri.compare(0, 10, "mongodb://") == 0) { + // don't delete anything here, instead have a cache based on the database id:s + // use boost split to split separate the uri into database / collection / id + + if (kind == uri_kind::mongodb && loaded_mongodb_results > 20) { // TODO: hard coded for now, let's revisit + path_scores.back().first.push_back(boost::filesystem::path()); + continue; + } + + std::string db_uri = uri.substr(10, uri.size()-10); + std::vector strs; + boost::split(strs, db_uri, boost::is_any_of("/")); + + // how do we get the home directory correctly? + boost::filesystem::path temp_path = boost::filesystem::absolute(boost::filesystem::path("quasimodo/temp") / strs[2] / "surfel_map.pcd"); + + if (kind == uri_kind::all && boost::filesystem::exists(temp_path.parent_path())) { + path_scores.back().first.push_back(temp_path.parent_path() / "convex_segments" / "segment.pcd"); + ++loaded_mongodb_results; + continue; + } + + if (!boost::filesystem::exists(temp_path.parent_path())) { + boost::filesystem::create_directories(temp_path.parent_path()); + boost::filesystem::create_directory(temp_path.parent_path() / "convex_segments"); + } + + boost::shared_ptr message; + mongo::BSONObj bson_message; + std::tie(message, bson_message) = message_store->queryID(strs[2]); + SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT); + pcl::fromROSMsg(message->surfel_cloud, *surfel_cloud); + pcl::io::savePCDFileBinary(temp_path.string(), *surfel_cloud); + pcl::io::savePCDFileBinary((temp_path.parent_path() / "convex_segments" / "segment.pcd").string(), *surfel_cloud); + + if (kind == uri_kind::mongodb && !message->removed_at.empty()) { + path_scores.back().first.push_back(boost::filesystem::path()); + } + else { + path_scores.back().first.push_back(temp_path.parent_path() / "convex_segments" / "segment.pcd"); + ++loaded_mongodb_results; + } + + // [0] - database, [1] - collection, [2] - mongodb object id + + } + else { + std::cout << "Loading from segment URI problem: URI not well formed!" << std::endl; + exit(-1); + } + + + } + + if (message_store != NULL) { // redundant but clearer + delete message_store; + } + + if (verbose) { + std::cout << "Exiting get_retrieved_path_scores" << std::endl; + } + + return path_scores; + +#else + std::vector, grouped_vocabulary_tree::result_type> > path_scores; int temp = 0; @@ -292,7 +455,13 @@ get_retrieved_path_scores(const std::vector::r ++temp; } + if (verbose) { + std::cout << "Exiting get_retrieved_path_scores" << std::endl; + } + return path_scores; + +#endif } template @@ -364,9 +533,30 @@ query_vocabulary(HistCloudT::Ptr& features, size_t nbr_query, grouped_vocabulary // add some common methods in vt for querying, really only one! std::vector::result_type> scores; - vt.query_vocabulary(scores, features, nbr_query); + if (kind == uri_kind::mongodb) { + vt.query_vocabulary(scores, features, 300); // TODO: make this a constant! + } + else { + vt.query_vocabulary(scores, features, nbr_query); + } - return get_retrieved_path_scores(scores, summary, verbose, kind); + using path_type = path_result >::type; + using result_type = std::pair::result_type>; + std::vector results = get_retrieved_path_scores(scores, summary, verbose, kind); + results.erase(std::remove_if(results.begin(), results.end(), [&kind](const result_type& r) { + if (r.first.empty()) { + return true; + } + boost::filesystem::path segment_path = r.first[0]; + std::string name = segment_path.stem().string(); + if (kind == uri_kind::mongodb && name != "segment") { // for the mongodb results + return true; + } + return false; + }), results.end()); + results.resize(std::min(nbr_query, results.size())); + + return results; } void insert_index_score(std::vector >& weighted_indices, const vocabulary_tree::result_type& index, float score) diff --git a/dynamic_object_retrieval/k_means_tree/impl/grouped_vocabulary_tree.hpp b/dynamic_object_retrieval/k_means_tree/impl/grouped_vocabulary_tree.hpp index 2ab39dfb..223fc910 100644 --- a/dynamic_object_retrieval/k_means_tree/impl/grouped_vocabulary_tree.hpp +++ b/dynamic_object_retrieval/k_means_tree/impl/grouped_vocabulary_tree.hpp @@ -73,8 +73,15 @@ void grouped_vocabulary_tree::query_vocabulary(vector& up vector selected_indices; // get<1>(scores[i])) is actually the index within the group! - double score = super::compute_min_combined_dist(selected_indices, query_cloud, vectors, adjacencies, - mapping, inverse_mapping, scores[i].subgroup_index); + double score; + if (adjacencies.empty()) { + score = scores[i].score; + selected_indices.push_back(0); + } + else { + score = super::compute_min_combined_dist(selected_indices, query_cloud, vectors, adjacencies, + mapping, inverse_mapping, scores[i].subgroup_index); + } //double score = scores[i].score; //selected_indices.push_back(scores[i].subgroup_index); updated_scores.push_back(result_type(float(score), scores[i].group_index, selected_indices[0])); @@ -105,6 +112,9 @@ void grouped_vocabulary_tree::query_vocabulary(vector& up vector > index_scores; int current_index = -1; for (size_t i = 0; i < updated_scores.size(); ++i) { + if (updated_scores[i].group_index == 100000) { + continue; + } if (updated_scores[i].group_index != current_index) { current_index = updated_scores[i].group_index; std::sort(index_scores.begin(), index_scores.end(), [](const pair& s1, const pair& s2) @@ -281,6 +291,10 @@ template void grouped_vocabulary_tree::load_cached_vocabulary_vectors_for_group(vector& vectors, set >& adjacencies, int i) { + if (i == 100000) { // this is hardcoded atm, but segments that don't have adjacencies should use this index + return; + } + boost::filesystem::path cache_path = boost::filesystem::path(save_state_path) / "vocabulary_vectors"; stringstream ss; diff --git a/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp b/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp index 84f21f71..7fbce3f3 100644 --- a/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp +++ b/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp @@ -299,6 +299,7 @@ void vocabulary_tree::source_freqs_for_node(std::map& source } } +// we should make some const here to make clear what happens template double vocabulary_tree::compute_min_combined_dist(vector& included_indices, CloudPtrT& cloud, vector& smaller_freqs, set >& adjacencies, map& mapping, map& inverse_mapping, int hint) // TODO: const From 295ce0d0e8f2c017f420e1a4b4fdbcceab61364a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 25 Aug 2016 17:18:04 +0200 Subject: [PATCH 076/255] memleeks, deps and segmentation --- .../simple_dynamic_object_parser.h | 4 +- quasimodo/quasimodo_brain/CMakeLists.txt | 2 +- quasimodo/quasimodo_brain/package.xml | 8 +- quasimodo/quasimodo_brain/src/Util/Util.cpp | 2 +- .../metaroom_additional_view_processing.cpp | 334 +++++++++- quasimodo/quasimodo_models/CMakeLists.txt | 6 +- .../registration/MassRegistrationPPR2.h | 44 +- .../src/modelupdater/ModelUpdater.cpp | 616 ++++++++---------- .../src/registration/MassRegistrationPPR2.cpp | 610 ++++++++--------- quasimodo/quasimodo_retrieval/CMakeLists.txt | 4 +- quasimodo/quasimodo_retrieval/package.xml | 4 +- 11 files changed, 938 insertions(+), 696 deletions(-) diff --git a/metaroom_xml_parser/include/metaroom_xml_parser/simple_dynamic_object_parser.h b/metaroom_xml_parser/include/metaroom_xml_parser/simple_dynamic_object_parser.h index 4d4724fd..28b3c27a 100644 --- a/metaroom_xml_parser/include/metaroom_xml_parser/simple_dynamic_object_parser.h +++ b/metaroom_xml_parser/include/metaroom_xml_parser/simple_dynamic_object_parser.h @@ -116,7 +116,7 @@ class SimpleDynamicObjectParser { } if (attributes.hasAttribute("roomRunNumber")) { - toRet.m_roomRunNumber = attributes.value("roomRunNumber").toString().toInt(); + toRet.m_roomRunNumber = atoi(attributes.value("roomRunNumber").toString().toStdString().c_str());//.toInt(); } else { std::cerr<<"Object xml node does not have roomRunNumber attribute."<libqt4-dev libceres-dev roscpp - soma2_msgs - soma_manager + soma_msgs + message_runtime eigen_conversions cv_bridge @@ -68,8 +68,8 @@ libqt4 libceres-dev roscpp - soma2_msgs - soma_manager + soma_msgs + message_runtime eigen_conversions cv_bridge diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index d36128ee..5cc02df1 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -316,7 +316,7 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec masks.push_back(mask); } - //mu->computeMovingDynamicStatic(bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions + //mu->computeMovingDynamicStatic(bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions // mu->computeMovingDynamicStatic(bgcp,bgcf,mod_po_vec[j],mod_fr_vec[j],debugg);//Determine self occlusions std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,debugg);//Determine self occlusions diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 60148a29..89366f57 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -408,10 +408,10 @@ reglib::Model * processAV(std::string path){ } reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path); -// for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ -// sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; -// sweep->relativeposes[i] = sweepPoses[i]; -// } + for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ + sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; + sweep->relativeposes[i] = sweepPoses[i]; + } std::vector frames; std::vector masks; @@ -458,7 +458,7 @@ reglib::Model * processAV(std::string path){ sweep->points = mu->getSuperPoints(sweep->relativeposes,sweep->frames,sweep->modelmasks,1,false); //Not needed if metaroom well calibrated - reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.15); bgmassreg->timeout = 20; bgmassreg->viewer = viewer; bgmassreg->use_surface = true; @@ -471,6 +471,7 @@ reglib::Model * processAV(std::string path){ bgmassreg->addModel(sweep); bgmassreg->setData(frames,masks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); + delete bgmassreg; for(unsigned int i = 0; i < frames.size(); i++){ frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; @@ -486,7 +487,7 @@ reglib::Model * processAV(std::string path){ fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); } - delete bgmassreg; + delete reg; delete mu; delete sweep; @@ -617,7 +618,11 @@ void processMetaroom(std::string path){ std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; - quasimodo_brain::segment(bg,models,internal,external,dynamic,true); + quasimodo_brain::segment(bg,models,internal,external,dynamic,false); + + bg->fullDelete(); + delete bg; + remove_old_seg(sweep_folder); for(unsigned int i = 0; i < models.size(); i++){ @@ -845,7 +850,8 @@ void processMetaroom(std::string path){ } - + fullmodel->fullDelete(); + delete fullmodel; // delete bgmassreg; delete reg; delete mu; @@ -893,12 +899,318 @@ void trainMetaroom(std::string path){ delete bgmassreg; } +std::vector loadModels(std::string path){ + printf("loadModels: %s\n",path.c_str()); + std::vector models; + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + std::vector frames; + std::vector poses; + readViewXML(sweep_folder+"ViewGroup.xml",frames,poses); + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_object*.xml")); + for (auto objectFile : objectFiles){ + std::string object = sweep_folder+objectFile.toStdString(); + printf("object: %s\n",object.c_str()); + + QFile file(object.c_str()); + + if (!file.exists()){ + ROS_ERROR("Could not open file %s to masks.",object.c_str()); + continue; + } + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()) + { + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()) + { + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + return; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement) + { + + if (xmlReader->name() == "View") + { + cv::Mat rgb; + cv::Mat depth; + //printf("elementName: %s\n",elementName.toStdString().c_str()); + QXmlStreamAttributes attributes = xmlReader->attributes(); + if (attributes.hasAttribute("RGB")) + { + QString rgbpath = attributes.value("RGB").toString(); + //printf("rgb filename: %s\n",rgbpath.toStdString().c_str()); + rgb = cv::imread(rgbpath.toStdString().c_str(), CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + if (attributes.hasAttribute("DEPTH")) + { + QString depthpath = attributes.value("DEPTH").toString(); + //printf("depth filename: %s\n",depthpath.toStdString().c_str()); + depth = cv::imread(depthpath.toStdString().c_str(), CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + token = xmlReader->readNext();//Stamp + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//sec + elementName = xmlReader->name().toString(); + int sec = atoi(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//nsec + elementName = xmlReader->name().toString(); + int nsec = atoi(xmlReader->readElementText().toStdString().c_str()); + token = xmlReader->readNext();//end stamp + + token = xmlReader->readNext();//Camera + elementName = xmlReader->name().toString(); + + reglib::Camera * cam = new reglib::Camera(); + + token = xmlReader->readNext();//fx + cam->fx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//fy + cam->fy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cx + cam->cx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cy + cam->cy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Camera + elementName = xmlReader->name().toString(); + + double time = double(sec)+double(nsec)/double(1e9); + + token = xmlReader->readNext();//RegisteredPose + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d regpose = getPose(xmlReader); + + token = xmlReader->readNext();//RegisteredPose + elementName = xmlReader->name().toString(); + + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d pose = getPose(xmlReader); + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, pose); + frames.push_back(frame); + poses.push_back(pose); + } + } + } +*/ + delete xmlReader; + +// QXmlStreamWriter* xmlWriter = new QXmlStreamWriter(); +// xmlWriter->setDevice(&file); + +// xmlWriter->writeStartDocument(); +// xmlWriter->writeStartElement("Object"); +// xmlWriter->writeAttribute("object_number", QString::number(d)); + +// for(unsigned int j = 0; j < mod_fr.size(); j++){ +// if(inds[j].size() == 0){continue;} +// reglib::RGBDFrame * frame = mod_fr[j]; +// reglib::Camera * cam = frame->camera; +// cv::Mat mask; +// mask.create(cam->height,cam->width,CV_8UC1); +// unsigned char * maskdata = (unsigned char *)(mask.data); +// for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 0;} +// for(unsigned int k = 0; k < inds[j].size(); k++){maskdata[inds[j][k]] = 255;} + +// char buf [1024]; +// sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),d,j); +// cv::imwrite(buf, mask ); +// xmlWriter->writeStartElement("Mask"); +// xmlWriter->writeAttribute("filename", QString(buf)); +// xmlWriter->writeAttribute("image_number", QString::number(j)); +// xmlWriter->writeEndElement(); +// } + +// xmlWriter->writeEndElement(); +// xmlWriter->writeEndDocument(); +// delete xmlWriter; + + models.push_back(mod); + + } + +// std::vector rgbs; +// std::vector depths; + +// for(int imgnr = 0; true; imgnr++){ +// char buf [1024]; + +// sprintf(buf,"%s/view_RGB%10.10i.png",sweep_folder.c_str(),imgnr); +// cv::Mat rgb = cv::imread(buf, CV_LOAD_IMAGE_UNCHANGED); + +// sprintf(buf,"%s/view_DEPTH%10.10i.png",sweep_folder.c_str(),imgnr); +// cv::Mat depth = cv::imread(buf, CV_LOAD_IMAGE_UNCHANGED); +// if(!rgb.data || !depth.data){break;} + +// rgbs.push_back(rgb); +// depths.push_back(depth); + +// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); +// cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", depth); +// cv::waitKey(0); +// } + + return models; +/* + + + std::vector viewrgbs; + std::vector viewdepths; + std::vector viewtfs; + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); + for (auto objectFile : objectFiles){ + auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); + for (unsigned int i=0; iheight,cloud->width,CV_8UC3); + unsigned char * rgbdata = (unsigned char *)rgb.data; + + cv::Mat depth; + depth.create(cloud->height,cloud->width,CV_16UC1); + unsigned short * depthdata = (unsigned short *)depth.data; + + unsigned int nr_data = cloud->height * cloud->width; + for(unsigned int j = 0; j < nr_data; j++){ + PointType p = cloud->points[j]; + rgbdata[3*j+0] = p.b; + rgbdata[3*j+1] = p.g; + rgbdata[3*j+2] = p.r; + depthdata[j] = short(5000.0 * p.z); + } + + viewrgbs.push_back(rgb); + viewdepths.push_back(depth); + viewtfs.push_back(object.vAdditionalViewsTransforms[i]); + } + } + + reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path); + for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ + sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; + sweep->relativeposes[i] = sweepPoses[i]; + } + + std::vector frames; + std::vector masks; + std::vector unrefined; + + std::vector both_unrefined; + both_unrefined.push_back(Eigen::Matrix4d::Identity()); + std::vector times; + for(unsigned int i = 0; i < viewrgbs.size(); i++){ + printf("additional view: %i\n",i); + geometry_msgs::TransformStamped msg; + tf::transformStampedTFToMsg(viewtfs[i], msg); + long sec = msg.header.stamp.sec; + long nsec = msg.header.stamp.nsec; + double time = double(sec)+1e-9*double(nsec); + + Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]); + + cout << m << endl << endl; + + unrefined.push_back(m); + times.push_back(time); + + cv::Mat fullmask; + fullmask.create(480,640,CV_8UC1); + unsigned char * maskdata = (unsigned char *)fullmask.data; + for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} + masks.push_back(new reglib::ModelMask(fullmask)); + + reglib::Camera * cam = sweep->frames.front()->camera->clone(); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m);//a.matrix()); + frames.push_back(frame); + + //both_unrefined.push_back(sweep->frames.front()->pose.inverse()*a.matrix()); + both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); + } + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( sweep, reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + sweep->points = mu->getSuperPoints(sweep->relativeposes,sweep->frames,sweep->modelmasks,1,false); + + //Not needed if metaroom well calibrated + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); + bgmassreg->timeout = 20; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = false; + bgmassreg->visualizationLvl = 0; + bgmassreg->maskstep = 5; + bgmassreg->nomaskstep = 5; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->addModel(sweep); + bgmassreg->setData(frames,masks); + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); + + for(unsigned int i = 0; i < frames.size(); i++){ + frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; + } + + reglib::Model * fullmodel = new reglib::Model(); + fullmodel->frames = sweep->frames; + fullmodel->relativeposes = sweep->relativeposes; + fullmodel->modelmasks = sweep->modelmasks; + for(unsigned int i = 0; i < frames.size(); i++){ + fullmodel->frames.push_back(frames[i]); + fullmodel->modelmasks.push_back(masks[i]); + fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); + } + + delete bgmassreg; + delete reg; + delete mu; + delete sweep; + + return fullmodel; + */ +} + int main(int argc, char** argv){ ros::init(argc, argv, "metaroom_additional_view_processing"); ros::NodeHandle n; - - int inputstate = 0; for(int i = 1; i < argc;i++){ printf("input: %s\n",argv[i]); @@ -935,6 +1247,8 @@ int main(int argc, char** argv){ //exit(0); }else{ out_pub = n.advertise(outtopic, 1000); + std::vector mods = loadModels(std::string(argv[i])); + exit(0); processMetaroom(std::string(argv[i])); } } diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 0eee28df..b764412f 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -94,6 +94,6 @@ install(TARGETS quasimodo_weightlib quasimodo_core quasimodo_reglib quasimodo_ma RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +#install(DIRECTORY launch +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +#) diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h index 67d46d12..236e32ed 100644 --- a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h +++ b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h @@ -10,11 +10,11 @@ namespace reglib class TestMatch{ public: - int src; - int dst; + long src; + long dst; double d; - TestMatch(int src_, int dst_, double d_){ + TestMatch(long src_, long dst_, double d_){ src = src_; dst = dst_; d = d_; @@ -47,11 +47,11 @@ namespace reglib double solve_equation_time; double total_time_start; - const unsigned int maxcount = 1000000; + const unsigned long maxcount = 1000000; Model * model; - std::vector nr_datas; + std::vector nr_datas; std::vector< bool > is_ok; @@ -64,7 +64,7 @@ namespace reglib - std::vector< int > kp_nr_arraypoints; + std::vector< long > kp_nr_arraypoints; std::vector< double * > kp_arraypoints; std::vector< double * > kp_arraynormals; std::vector< double * > kp_arrayinformations; @@ -76,20 +76,20 @@ namespace reglib double * kp_Xn_arr; double * kp_rangeW_arr; DistanceWeightFunction2PPR2 * kpfunc; - //std::vector< std::vector< std::vector > > matchids; + //std::vector< std::vector< std::vector > > matchids; - std::vector< int > frameid; + std::vector< long > frameid; bool use_surface; - std::vector< int > nr_arraypoints; + std::vector< long > nr_arraypoints; std::vector< double * > arraypoints; std::vector< double * > arraynormals; std::vector< double * > arraycolors; std::vector< double * > arrayinformations; std::vector< Tree3d * > trees3d; std::vector< ArrayData3D * > a3dv; - std::vector nr_matches; - std::vector< std::vector< std::vector > > matchids; + std::vector nr_matches; + std::vector< std::vector< std::vector > > matchids; std::vector< std::vector< std::vector > > matchdists; double * Qp_arr; double * Qn_arr; @@ -101,23 +101,23 @@ namespace reglib bool use_depthedge; - //int depthedge_nr_neighbours; - std::vector< int > depthedge_nr_arraypoints; + //long depthedge_nr_neighbours; + std::vector< long > depthedge_nr_arraypoints; std::vector< double * > depthedge_arraypoints; - //std::vector< int * > depthedge_neighbours; + //std::vector< long * > depthedge_neighbours; std::vector< double * > depthedge_arrayinformations; std::vector< Tree3d * > depthedge_trees3d; std::vector< ArrayData3D * > depthedge_a3dv; - std::vector depthedge_nr_matches; - std::vector< std::vector< std::vector > > depthedge_matchids; + std::vector depthedge_nr_matches; + std::vector< std::vector< std::vector > > depthedge_matchids; std::vector< std::vector< std::vector > > depthedge_matchdists; double * depthedge_Qp_arr; double * depthedge_Xp_arr; double * depthedge_rangeW_arr; DistanceWeightFunction2PPR2 * depthedge_func; - std::vector sweepids; - std::vector background_nr_datas; + std::vector sweepids; + std::vector background_nr_datas; // std::vector< Eigen::Matrix > background_points; // std::vector< Eigen::Matrix > background_colors; // std::vector< Eigen::Matrix > background_normals; @@ -125,12 +125,12 @@ namespace reglib // std::vector< Eigen::Matrix > background_transformed_normals; // std::vector< Eigen::VectorXd > background_informations; // std::vector< nanoflann::KDTreeAdaptor * > background_trees; -// std::vector background_nr_matches; -// std::vector< std::vector< std::vector > > background_matchids; +// std::vector background_nr_matches; +// std::vector< std::vector< std::vector > > background_matchids; -// std::vector feature_start;//Dimension of data a specific feature starts, if the feature is RGB this should be 3 -// std::vector feature_end;//Dimension of data a specific feature ends, if the feature is RGB this should be 5 +// std::vector feature_start;//Dimension of data a specific feature starts, if the feature is RGB this should be 3 +// std::vector feature_end;//Dimension of data a specific feature ends, if the feature is RGB this should be 5 // std::vector< DistanceWeightFunction2 * > feature_func; diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 3701baf5..4fa77ebd 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1805,286 +1805,272 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d } std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ - cv::Mat src = frame->rgb.clone(); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); - const float idepth = frame->camera->idepth_scale; + cv::Mat src = frame->rgb.clone(); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + const float idepth = frame->camera->idepth_scale; - cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); + cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); - unsigned char * srcdata = (unsigned char *)src.data; - unsigned int width = src.cols; - unsigned int height = src.rows; - unsigned int nr_pixels = width*height; - unsigned int nr_rgbedges = (width-2)*height + width*(height-2) + 2*(width-2)*(height-2); + unsigned char * srcdata = (unsigned char *)src.data; + unsigned int width = src.cols; + unsigned int height = src.rows; + std::vector dxc; + dxc.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0;} - cv::Mat src_dx; - src_dx.create(height,width,CV_32FC1); - float * src_dxdata = (float *)(src_dx.data); + std::vector dyc; + dyc.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){dyc[i] = 0;} - cv::Mat src_dy; - src_dy.create(height,width,CV_32FC1); - float * src_dydata = (float *)(src_dy.data); + std::vector src_dxdata; + src_dxdata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){src_dxdata[i] = 0;} - cv::Mat maxima_dx; - maxima_dx.create(height,width,CV_8UC1); - unsigned char * maxima_dxdata = (unsigned char *)(maxima_dx.data); + std::vector src_dydata; + src_dydata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){src_dydata[i] = 0;} - cv::Mat maxima_dy; - maxima_dy.create(height,width,CV_8UC1); - unsigned char * maxima_dydata = (unsigned char *)(maxima_dy.data); + std::vector maxima_dxdata; + maxima_dxdata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){maxima_dxdata[i] = 0;} - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - src_dxdata[ind] = 0; - src_dydata[ind] = 0; - } - } + std::vector maxima_dydata; + maxima_dydata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){maxima_dydata[i] = 0;} - unsigned int chans = 3; - for(unsigned int c = 0; c < chans;c++){ - int dir; - for(unsigned int w = 1; w < width;w++){ - for(unsigned int h = 1; h < height;h++){ - int ind = h*width+w; - dir = 1; - src_dxdata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]) / 255.0)/3.0; - - dir = width; - src_dydata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c]) / 255.0)/3.0; - } - } - - } - - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - maxima_dxdata[ind] = 255*(src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] > src_dxdata[ind+1]); - maxima_dydata[ind] = 255*(src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] > src_dydata[ind+width]); - } - } + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + src_dxdata[ind] = 0; + src_dydata[ind] = 0; + } + } + unsigned int chans = 3; + for(unsigned int c = 0; c < chans;c++){ + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 1; h < height;h++){ + int ind = h*width+w; + int dir = 1; + src_dxdata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-1)+c]) / 255.0)/3.0; + dir = width; + src_dydata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-width)+c]) / 255.0)/3.0; + } + } + } - std::vector< std::vector > probs; - bool cross = false; - //unsigned int chans = 3; - for(unsigned int c = 0; c < chans;c++){ - std::vector Xvec; - int dir; - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - dir = 1; - Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + maxima_dxdata[ind] = (src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] > src_dxdata[ind+1]); + maxima_dydata[ind] = (src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] > src_dydata[ind+width]); + } + } - dir = width; - Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - } - } - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); - for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} - double stdval = 0; - for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} - stdval = sqrt(stdval/double(Xvec.size())); - DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); - func->zeromean = true; - func->maxp = 0.9999; - func->startreg = 0.5; - func->debugg_print = false; - func->bidir = true; - func->maxd = 256.0; - func->histogram_size = 256; - func->fixed_histogram_size = true; - func->startmaxd = func->maxd; - func->starthistogram_size = func->histogram_size; - func->blurval = 0.005; - func->stdval2 = stdval; - func->maxnoise = stdval; - func->reset(); - func->computeModel(X); - //printf("noise: %5.5f\n",func->getNoise()); - - std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} - std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} - std::vector dxy; dxy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dxy[i] = 0.5;} - std::vector dyx; dyx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dyx[i] = 0.5;} - for(unsigned int w = 1; w < width;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; + std::vector< std::vector > probs; - dir = 1; - dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + for(unsigned int c = 0; c < chans;c++){ + std::vector Xvec; + int dir; + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + dir = 1; + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - dir = width; - dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - } - } - delete func; - - probs.push_back(dx); - probs.push_back(dy); - } - - { - std::vector Xvec; - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); + dir = width; + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + } + } - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} - } + DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + func->zeromean = true; + func->maxp = 0.9999; + func->startreg = 0.5; + func->debugg_print = false; + func->bidir = true; + func->maxd = 256.0; + func->histogram_size = 256; + func->fixed_histogram_size = true; + func->startmaxd = func->maxd; + func->starthistogram_size = func->histogram_size; + func->blurval = 0.005; + func->stdval2 = stdval; + func->maxnoise = stdval; + func->reset(); + func->computeModel(X); + + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + + dir = 1; + dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = width; + dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + } + } + delete func; - if(h > 1){ - int dir = -width; - int other2 = ind+2*dir; - int other = ind+dir; + probs.push_back(dx); + probs.push_back(dy); + } - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; + { + std::vector Xvec; + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} - } - } - } + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); - for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} - - double stdval = 0; - for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} - stdval = sqrt(stdval/double(Xvec.size())); - - DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); - funcZ->zeromean = true; - funcZ->startreg = 0.002; - funcZ->debugg_print = false; - funcZ->bidir = true; - funcZ->maxp = 0.999999; - funcZ->maxd = 0.1; - funcZ->histogram_size = 100; - funcZ->fixed_histogram_size = true; - funcZ->startmaxd = funcZ->maxd; - funcZ->starthistogram_size = funcZ->histogram_size; - funcZ->blurval = 0.5; - funcZ->stdval2 = stdval; - funcZ->maxnoise = stdval; - funcZ->reset(); - funcZ->computeModel(X); - - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} - } + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; - if(h > 1){ - int dir = -width; - int other2 = ind+2*dir; - int other = ind+dir; + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + } + } - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); + funcZ->zeromean = true; + funcZ->startreg = 0.002; + funcZ->debugg_print = false; + funcZ->bidir = true; + funcZ->maxp = 0.999999; + funcZ->maxd = 0.1; + funcZ->histogram_size = 100; + funcZ->fixed_histogram_size = true; + funcZ->startmaxd = funcZ->maxd; + funcZ->starthistogram_size = funcZ->histogram_size; + funcZ->blurval = 0.5; + funcZ->stdval2 = stdval; + funcZ->maxnoise = stdval; + funcZ->reset(); + funcZ->computeModel(X); + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} - } - } - } + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; - std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} - std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + } + } - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; - float dz = fabs(z-z2); - if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} - if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} - } - if(h > 1){ - int dir = -width; + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); - int other2 = ind+2*dir; - int other = ind+dir; + float dz = fabs(z-z2); + if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} + if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + } - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); + if(h > 1){ + int dir = -width; - float dz = fabs(z-z2); - if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} - if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} - } - } - } + int other2 = ind+2*dir; + int other = ind+dir; - delete funcZ; - probs.push_back(dx); - probs.push_back(dy); - } + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + float dz = fabs(z-z2); + if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} + if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + } + } + } - cv::Mat col_probs; - col_probs.create(height,width,CV_32FC3); - float * cpdata = (float *)col_probs.data; + delete funcZ; + probs.push_back(dx); + probs.push_back(dy); + } - cv::Mat d_probs; - d_probs.create(height,width,CV_32FC3); - float * dpdata = (float *)d_probs.data; for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; - cpdata[3*ind+0] = 0; - cpdata[3*ind+1] = 0; - cpdata[3*ind+2] = 0; - - dpdata[3*ind+0] = 0; - dpdata[3*ind+1] = 0; - dpdata[3*ind+2] = 0; + float probX = 0; + float probY = 0; if(w > 0 && w < width-1){ float ax = 0.5; @@ -2095,17 +2081,13 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int bx *= 1.0-pr; } float px = ax/(ax+bx); - cpdata[3*ind+1] = (1-px) * float(maxima_dxdata[ind] > 0); - dpdata[3*ind+1] = (1-probs[probs.size()-2][ind]); - if(!frame->det_dilate.data[ind]){dpdata[3*ind+1] = (1-px) * float(maxima_dxdata[ind] > 0);} - else{ - dpdata[3*ind+1] = std::max(dpdata[3*ind+1],0.8f*(1-px) * float(maxima_dxdata[ind] > 0)); - } - //dpdata[3*ind+1] *= cpdata[3*ind+1]; + float current = 0; + if(!frame->det_dilate.data[ind]){ current = (1-px) * float(maxima_dxdata[ind]);} + else{ current = std::max(float(1-probs[probs.size()-2][ind]),0.99f*(1-px) * float(maxima_dxdata[ind]));} + probX = 1-current; } if(h > 0 && h < height-1){ - float ay = 0.5; float by = 0.5; for(unsigned int p = 1; p < probs.size()-2; p+=2){ @@ -2114,113 +2096,37 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int by *= 1.0-pr; } float py = ay/(ay+by); - cpdata[3*ind+2] = (1-py) * float(maxima_dydata[ind] > 0); - dpdata[3*ind+2] = (1-probs[probs.size()-1][ind]); - if(!frame->det_dilate.data[ind]){dpdata[3*ind+2] = (1-py) * float(maxima_dydata[ind] > 0);} - else{ - dpdata[3*ind+2] = std::max(dpdata[3*ind+2],0.8f*(1-py) * float(maxima_dydata[ind] > 0)); - } - double maxw = std::max(dpdata[3*ind+1],dpdata[3*ind+2]); - dpdata[3*ind+1] = maxw; - dpdata[3*ind+2] = maxw; - //dpdata[3*ind+2] *= cpdata[3*ind+2]; + float current = 0; + if(!frame->det_dilate.data[ind]){ current = (1-py) * float(maxima_dydata[ind]);} + else{ current = std::max(float(1-probs[probs.size()-1][ind]),0.99f*(1-py) * float(maxima_dydata[ind]));} + probY = 1-current; } + + dxc[ind] = std::min(probX,probY); + dyc[ind] = std::min(probX,probY); } } -// cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); -// cv::namedWindow( "col_probs", cv::WINDOW_AUTOSIZE ); cv::imshow( "col_probs", col_probs); -// cv::namedWindow( "d_probs", cv::WINDOW_AUTOSIZE ); cv::imshow( "d_probs", d_probs); -// // cv::namedWindow( "src_dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_dx", cv::abs(src_dx)); -// // cv::namedWindow( "src_dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_dy", cv::abs(src_dy)); -// // cv::namedWindow( "maxima_dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "maxima_dx",maxima_dx); -// // cv::namedWindow( "maxima_dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "maxima_dy",maxima_dy); - -// cv::waitKey(0); - - //return probs; - - std::vector dxc; - dxc.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){dxc[i] = 1-dpdata[3*i+1];} - - std::vector dyc; - dyc.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){dyc[i] = 1-dpdata[3*i+2];} - - std::vector< std::vector > probs2; probs2.push_back(dxc); probs2.push_back(dyc); - return probs2; -/* - std::vector dxc; - dxc.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0.5;} - - std::vector dyc; - dyc.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){dyc[i] = 0.5;} - - cv::Mat col_probs; - col_probs.create(height,width,CV_32FC3); - float * cpdata = (float *)col_probs.data; + cv::Mat edges; + edges.create(height,width,CV_32FC3); + float * edgesdata = (float *)edges.data; - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - - cpdata[3*ind+0] = 0; - cpdata[3*ind+1] = 0; - cpdata[3*ind+2] = 0; - - if(w > 0 && w < width-1){ - double ax = 0.5; - double bx = 0.5; - for(unsigned int p = 0; p < probs.size()-2; p+=2){ - double pr = probs[p][ind]; - ax *= pr; - bx *= 1.0-pr; - } - double px = ax/(ax+bx); - cpdata[3*ind+1] = (1-px) * float(maxima_dxdata[ind] > 0); - dxc[ind] = 1-cpdata[3*ind+1]; - } - - if(h > 0 && h < height-1){ + for(unsigned int i = 0; i < width*height;i++){ + edgesdata[3*i+0] = 0; + edgesdata[3*i+1] = dxc[i]; + edgesdata[3*i+2] = dyc[i]; + } - double ay = 0.5; - double by = 0.5; - for(unsigned int p = 1; p < probs.size()-2; p+=2){ - double pr = probs[p][ind]; - ay *= pr; - by *= 1.0-pr; - } - double py = ay/(ay+by); - cpdata[3*ind+2] = (1-py) * float(maxima_dydata[ind] > 0); - dyc[ind] = 1-cpdata[3*ind+2]; - } - } - } +// cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); +// cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); +// cv::waitKey(0); - std::vector< std::vector > probs2; - probs2.push_back(dxc); - probs2.push_back(dyc); - probs2.push_back(probs[probs.size()-2]); - probs2.push_back(probs.back()); - - // cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); - // cv::namedWindow( "col_probs", cv::WINDOW_AUTOSIZE ); cv::imshow( "col_probs", col_probs); - // cv::namedWindow( "src_dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_dx", cv::abs(src_dx)); - // cv::namedWindow( "src_dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_dy", cv::abs(src_dy)); - // cv::namedWindow( "maxima_dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "maxima_dx",maxima_dx); - // cv::namedWindow( "maxima_dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "maxima_dy",maxima_dy); - - // cv::waitKey(0); - return probs2; - */ + return probs2; } std::vector doInference(std::vector & prior, std::vector< std::vector > & connectionId, std::vector< std::vector > & connectionStrength){ @@ -2248,7 +2154,7 @@ std::vector doInference(std::vector & prior, std::vector< std::vect for(unsigned int i = 0; i < nr_data;i++){ for(unsigned int j = 0; j < connectionId[i].size();j++){ - double weight = connectionStrength[i][j]; + double weight = 10*connectionStrength[i][j]; g -> add_edge( i, connectionId[i][j], weight, weight ); //try{g -> add_edge( i, connectionId[i][j], weight, weight );} //catch(std::exception ex){printf("%i %i -> weight: %20.20f\n",i,connectionId[i],weight);exit(0);} @@ -2274,6 +2180,10 @@ void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * ove unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); float * src_normalsdata = (float *)(frame1->normals.data); + + unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); + unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); + Camera * src_camera = frame1->camera; const unsigned int src_width = src_camera->width; const unsigned int src_height = src_camera->height; @@ -2372,6 +2282,8 @@ void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * ove for(unsigned int src_w = 0; src_w < src_width;src_w++){ for(unsigned int src_h = 0; src_h < src_height;src_h++){ int src_ind = src_h*src_width+src_w; + + float src_z = src_idepth*float(src_depthdata[src_ind]); float src_nx = src_normalsdata[3*src_ind+0]; if(src_z > 0 && src_nx != 2){ @@ -2382,9 +2294,11 @@ void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * ove float src_y = (float(src_h) - src_cy) * src_z * src_ify; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + if(tz <= 0){continue;} + float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; float itz = 1.0/tz; float dst_w = dst_fx*tx*itz + dst_cx; float dst_h = dst_fy*ty*itz + dst_cy; @@ -2408,12 +2322,13 @@ void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * ove float tny = m10*src_nx + m11*src_ny + m12*src_nz; float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); - //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); - //d *= src_z;//compare_mul; + double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / sqrt(dst_noise*dst_noise + point_noise*point_noise); double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + if(dst_detdata[dst_ind] != 0){continue;} + if(src_detdata[src_ind] != 0){continue;} + + if(fabs(d) < 0.02){//If close, according noises, and angle of the surfaces similar: FUSE if(surface_angle > 0.9){ overlaps[src_ind] = std::max(overlaps[src_ind],0.9); // overlaps[src_ind]++; @@ -2541,13 +2456,13 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcp, vectorpoints[i].r = estimates[3*i+0]*255.0; + cloud->points[i].g = estimates[3*i+1]*255.0; + cloud->points[i].b = estimates[3*i+2]*255.0; + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } + + ArrayData3D * a3d = new ArrayData3D; a3d->data = points; a3d->rows = current_point; diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 7ed43bab..fc93fc01 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -113,7 +113,7 @@ void MassRegistrationPPR2::addModel(Model * model){ nr_matches.push_back( 0); - matchids.push_back( std::vector< std::vector >() ); + matchids.push_back( std::vector< std::vector >() ); matchdists.push_back( std::vector< std::vector >() ); nr_datas.push_back( 0); @@ -134,7 +134,7 @@ void MassRegistrationPPR2::addModel(Model * model){ a3dv.push_back(0); depthedge_nr_matches.push_back( 0); - depthedge_matchids.push_back( std::vector< std::vector >() ); + depthedge_matchids.push_back( std::vector< std::vector >() ); depthedge_matchdists.push_back( std::vector< std::vector >() ); depthedge_nr_arraypoints.push_back(0); @@ -143,13 +143,13 @@ void MassRegistrationPPR2::addModel(Model * model){ depthedge_trees3d.push_back(0); depthedge_a3dv.push_back(0); - int step = maskstep*maskstep; + long step = maskstep*maskstep; is_ok.push_back(false); - int count = model->points.size()/step; - int i = points.size()-1; + long count = model->points.size()/step; + long i = points.size()-1; if(count < 10){ is_ok[i] = false; return; @@ -183,7 +183,7 @@ void MassRegistrationPPR2::addModel(Model * model){ Eigen::Matrix & tXn = transformed_normals[i]; Eigen::VectorXd information (count); - for(unsigned int c = 0; c < count; c++){ + for(unsigned long c = 0; c < count; c++){ ap[3*c+0] = model->points[c*step].point(0); ap[3*c+1] = model->points[c*step].point(1); @@ -222,10 +222,10 @@ void MassRegistrationPPR2::addModel(Model * model){ trees3d[i]->buildIndex(); /* - int depthedge_count = 0; - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height; h++){ - int ind = h*width+w; + long depthedge_count = 0; + for(unsigned long w = 0; w < width; w++){ + for(unsigned long h = 0; h < height; h++){ + long ind = h*width+w; if(maskvec[ind] && edgedata[ind] == 255){ float z = idepth*float(depthdata[ind]); if(z > 0.2){depthedge_count++;} @@ -241,10 +241,10 @@ void MassRegistrationPPR2::addModel(Model * model){ depthedge_arrayinformations[i] = depthedge_ai; c = 0; - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height; h++){ + for(unsigned long w = 0; w < width; w++){ + for(unsigned long h = 0; h < height; h++){ if(c == depthedge_count){continue;} - int ind = h*width+w; + long ind = h*width+w; if(maskvec[ind] && edgedata[ind] == 255){ float z = idepth*float(depthdata[ind]); if(z > 0.2){ @@ -274,13 +274,13 @@ void MassRegistrationPPR2::addModelData(Model * model_, bool submodels){ printf("addModelData\n"); if(submodels){ - for(unsigned int i = 0; i < model->submodels.size(); i++){ + for(unsigned long i = 0; i < model->submodels.size(); i++){ addData(model->submodels[i]->getPCLnormalcloud(1,false)); } }else{ //setData(model->frames,model->modelmasks); - for(unsigned int i = 0; i < model->submodels.size(); i++){ + for(unsigned long i = 0; i < model->submodels.size(); i++){ addData(model->submodels[i]->getPCLnormalcloud(1,false)); // kp_nr_arraypoints.push_back(0); // kp_arraypoints.push_back(0); @@ -290,9 +290,9 @@ void MassRegistrationPPR2::addModelData(Model * model_, bool submodels){ // frameid.push_back(-1); } - unsigned int nr_frames = model->frames.size(); + unsigned long nr_frames = model->frames.size(); - for(unsigned int i = 0; i < nr_frames; i++){ + for(unsigned long i = 0; i < nr_frames; i++){ // frameid.push_back(i); addData(model->frames[i], model->modelmasks[i]); @@ -306,24 +306,24 @@ void MassRegistrationPPR2::addModelData(Model * model_, bool submodels){ // unsigned short * depthdata = (unsigned short *)(frame->depth.data); // Camera * camera = frame->camera; -// const unsigned int width = camera->width; -// const unsigned int height = camera->height; +// const unsigned long width = camera->width; +// const unsigned long height = camera->height; // const float idepth = camera->idepth_scale; // const float cx = camera->cx; // const float cy = camera->cy; // const float ifx = 1.0/camera->fx; // const float ify = 1.0/camera->fy; -// unsigned int nr_kp = keypoints.size(); +// unsigned long nr_kp = keypoints.size(); // if(nr_kp){ // double * ap = new double[3*nr_kp]; // double * an = new double[3*nr_kp]; // double * ai = new double[nr_kp]; -// for(unsigned int k = 0; k < nr_kp; k++){ +// for(unsigned long k = 0; k < nr_kp; k++){ // cv::KeyPoint & kp = keypoints[k]; // double w = kp.pt.x; // double h = kp.pt.y; -// int ind = int(h+0.5)*width+int(w+0.5); +// long ind = int(h+0.5)*width+int(w+0.5); // float z = idepth*float(depthdata[ind]); // float x = (w - cx) * z * ifx; @@ -354,7 +354,7 @@ void MassRegistrationPPR2::addModelData(Model * model_, bool submodels){ // } } -// for(int i = 0; i < is_ok.size(); i++){ +// for(long i = 0; i < is_ok.size(); i++){ // if(is_ok[i]){printf("%i is ok\n",i); // }else{printf("%i is not ok\n",i);} // } @@ -366,20 +366,20 @@ void MassRegistrationPPR2::addModelData(Model * model_, bool submodels){ void MassRegistrationPPR2::setData(std::vector< pcl::PointCloud::Ptr > all_clouds){ double total_load_time_start = getTime(); - unsigned int nr_frames = all_clouds.size(); + unsigned long nr_frames = all_clouds.size(); if(arraypoints.size() > 0){ - for(unsigned int i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} + for(unsigned long i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} arraypoints.resize(0); } if(a3dv.size() > 0){ - for(unsigned int i = 0; i < a3dv.size(); i++){delete a3dv[i];} + for(unsigned long i = 0; i < a3dv.size(); i++){delete a3dv[i];} a3dv.resize(0); } if(trees3d.size() > 0){ - for(unsigned int i = 0; i < trees3d.size(); i++){delete trees3d[i];} + for(unsigned long i = 0; i < trees3d.size(); i++){delete trees3d[i];} trees3d.resize(0); } @@ -405,11 +405,11 @@ void MassRegistrationPPR2::setData(std::vector< pcl::PointCloud::Ptr cloud = all_clouds[i]; - int count = 0; - for(unsigned int i = 0; i < cloud->points.size(); i++){ + long count = 0; + for(unsigned long i = 0; i < cloud->points.size(); i++){ if (isValidPoint(cloud->points[i])){count++;} } @@ -447,8 +447,8 @@ void MassRegistrationPPR2::setData(std::vector< pcl::PointCloudpoints.size(); i++){ + long c = 0; + for(unsigned long i = 0; i < cloud->points.size(); i++){ pcl::PointXYZRGBNormal p = cloud->points[i]; if (isValidPoint(p)){ @@ -506,27 +506,27 @@ void MassRegistrationPPR2::setData(std::vector< pcl::PointCloud frames_,std::vector mmasks_){ double total_load_time_start = getTime(); - unsigned int nr_frames = frames_.size(); + unsigned long nr_frames = frames_.size(); // if(arraypoints.size() > 0){ -// for(unsigned int i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} +// for(unsigned long i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} // arraypoints.resize(0); // } // if(a3dv.size() > 0){ -// for(unsigned int i = 0; i < a3dv.size(); i++){delete a3dv[i];} +// for(unsigned long i = 0; i < a3dv.size(); i++){delete a3dv[i];} // a3dv.resize(0); // } // if(trees3d.size() > 0){ -// for(unsigned int i = 0; i < trees3d.size(); i++){delete trees3d[i];} +// for(unsigned long i = 0; i < trees3d.size(); i++){delete trees3d[i];} // trees3d.resize(0); // } - for(unsigned int i = 0; i < nr_frames; i++){addData(frames_[i], mmasks_[i]);} + for(unsigned long i = 0; i < nr_frames; i++){addData(frames_[i], mmasks_[i]);} //printf("total load time: %5.5f\n",getTime()-total_load_time_start); } -double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, int nr_ap, double * ap, std::vector & matchid, std::vector & matchdist, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ +double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ap, double * ap, std::vector & matchid, std::vector & matchdist, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); @@ -543,8 +543,8 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, int nr_a #if defdomultithread #pragma omp parallel for num_threads(8) #endif - for(unsigned int k = 0; k < nr_ap; ++k) { - int prev = matchid[k]; + for(unsigned long k = 0; k < nr_ap; ++k) { + long prev = matchid[k]; double qp [3]; const double & src_x = ap[3*k+0]; const double & src_y = ap[3*k+1]; @@ -565,7 +565,7 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, int nr_a bad++; } - int current = ret_index; + long current = ret_index; new_good_rematches += prev != current; new_total_rematches++; matchid[k] = current; @@ -575,7 +575,7 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, int nr_a return good/(good+bad+0.001); } -double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, int nr_ap, double * ap, std::vector & matchid, std::vector & matchdist, double * apj, int nr_dn, int * dn, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ +double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ap, double * ap, std::vector & matchid, std::vector & matchdist, double * apj, long nr_dn, long * dn, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); @@ -592,7 +592,7 @@ double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, i #if defdomultithread #pragma omp parallel for num_threads(8) #endif - for(unsigned int k = 0; k < nr_ap; ++k) { + for(unsigned long k = 0; k < nr_ap; ++k) { const double & src_x = ap[3*k+0]; const double & src_y = ap[3*k+1]; const double & src_z = ap[3*k+2]; @@ -601,7 +601,7 @@ double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, i const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; - int prev = matchid[k]; + long prev = matchid[k]; const double & dx1 =apj[3*prev+0]-sx; const double & dy1 =apj[3*prev+1]-sy; const double & dz1 =apj[3*prev+2]-sz; @@ -611,9 +611,9 @@ double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, i printf("point %4.4i before: %5.5f -> ",k,before); while(true){ - int next = prev; - for(unsigned int m = 0; m < nr_dn; ++m) { - int id = dn[nr_dn*prev+m]; + long next = prev; + for(unsigned long m = 0; m < nr_dn; ++m) { + long id = dn[nr_dn*prev+m]; const double & dx =apj[3*id+0]-sx; const double & dy =apj[3*id+1]-sy; const double & dz =apj[3*id+2]-sz; @@ -654,7 +654,7 @@ double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, i // bad++; // } -// int current = ret_index; +// long current = ret_index; // new_good_rematches += prev != current; // new_total_rematches++; // matchid[k] = current; @@ -668,21 +668,21 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect //printf("rematch\n"); double new_good_rematches = 0; double new_total_rematches = 0; - unsigned int nr_frames = poses.size(); + unsigned long nr_frames = poses.size(); - int rmt = 2; + long rmt = 2; if(rmt==2){ - int overlapping = 0; + long overlapping = 0; - int work_done = 0; - int ignores_motion = 0; - int ignores_overlap = 0; - for(unsigned int i = 0; i < nr_frames; i++){ + long work_done = 0; + long ignores_motion = 0; + long ignores_overlap = 0; + for(unsigned long i = 0; i < nr_frames; i++){ if(!is_ok[i]){continue;} nr_matches[i] = 0; - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} if(i == j){continue;} if((matchscores[i][j] <= 0.001) && (matchscores[j][i] <= 0.001) && (rand()%nr_frames > 1)){ @@ -698,9 +698,9 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect double change_trans = 0; double change_rot = 0; double dt = 0; - for(unsigned int k = 0; k < 3; k++){ + for(unsigned long k = 0; k < 3; k++){ dt += diff(k,3)*diff(k,3); - for(unsigned int l = 0; l < 3; l++){ + for(unsigned long l = 0; l < 3; l++){ if(k == l){ change_rot += fabs(1-diff(k,l));} else{ change_rot += fabs(diff(k,l));} } @@ -738,14 +738,14 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect } if(rmt==1){ - for(unsigned int i = 0; i < nr_frames; i++){ + for(unsigned long i = 0; i < nr_frames; i++){ if(!is_ok[i]){continue;} nr_matches[i] = 0; double * ap = arraypoints[i]; - const unsigned int nr_ap = nr_arraypoints[i]; + const unsigned long nr_ap = nr_arraypoints[i]; - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} if(i == j){continue;} Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); @@ -753,12 +753,12 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); - std::vector & matchid = matchids[i][j]; + std::vector & matchid = matchids[i][j]; matchid.resize(nr_ap); Tree3d * t3d = trees3d[j]; - for(unsigned int k = 0; k < nr_ap; ++k) { - int prev = matchid[k]; + for(unsigned long k = 0; k < nr_ap; ++k) { + long prev = matchid[k]; double qp [3]; const double & src_x = ap[3*k+0]; const double & src_y = ap[3*k+1]; @@ -773,7 +773,7 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect resultSet.init(&ret_index, &out_dist_sqr ); t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); - int current = ret_index; + long current = ret_index; new_good_rematches += prev != current; new_total_rematches++; matchid[k] = current; @@ -783,14 +783,14 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect } } if(rmt==0){ - for(unsigned int i = 0; i < nr_frames; i++){ + for(unsigned long i = 0; i < nr_frames; i++){ if(!is_ok[i]){continue;} nr_matches[i] = 0; double * ap = arraypoints[i]; - int nr_ap = nr_arraypoints[i]; + long nr_ap = nr_arraypoints[i]; - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} if(i == j){continue;} Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); @@ -800,13 +800,13 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect Eigen::Matrix tX = rp*points[i]; - unsigned int nr_data = nr_datas[i]; - std::vector & matchid = matchids[i][j]; + unsigned long nr_data = nr_datas[i]; + std::vector & matchid = matchids[i][j]; matchid.resize(nr_data); Tree3d * t3d = trees3d[j]; - for(unsigned int k = 0; k < nr_data; ++k) { - int prev = matchid[k]; + for(unsigned long k = 0; k < nr_data; ++k) { + long prev = matchid[k]; double * qp = tX.col(k).data(); size_t ret_index; double out_dist_sqr; @@ -814,7 +814,7 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect resultSet.init(&ret_index, &out_dist_sqr ); t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); - int current = ret_index; + long current = ret_index; new_good_rematches += prev != current; new_total_rematches++; matchid[k] = current; @@ -829,19 +829,19 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect Eigen::MatrixXd MassRegistrationPPR2::getAllResiduals(std::vector poses){ - unsigned int nr_frames = poses.size(); + unsigned long nr_frames = poses.size(); Eigen::MatrixXd all_residuals; - int total_matches = 0; - for(unsigned int i = 0; i < nr_frames; i++){ + long total_matches = 0; + for(unsigned long i = 0; i < nr_frames; i++){ if(!is_ok[i]){continue;} - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} total_matches += matchids[i][j].size(); } } - int all_residuals_type = 1; + long all_residuals_type = 1; if(all_residuals_type == 1){ switch(type) { @@ -850,16 +850,16 @@ Eigen::MatrixXd MassRegistrationPPR2::getAllResiduals(std::vector & matchidi = matchids[i][j]; - unsigned int matchesi = matchidi.size(); + std::vector & matchidi = matchids[i][j]; + unsigned long matchesi = matchidi.size(); Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); @@ -878,8 +878,8 @@ Eigen::MatrixXd MassRegistrationPPR2::getAllResiduals(std::vector= nr_apj){continue;} const double & src_x = api[3*ki+0]; const double & src_y = api[3*ki+1]; @@ -914,8 +914,8 @@ Eigen::MatrixXd MassRegistrationPPR2::getAllResiduals(std::vector & tXi = transformed_points[i]; Eigen::Matrix & tXni = transformed_normals[i]; Eigen::VectorXd & informationi = informations[i]; - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} if(i == j){continue;} - std::vector & matchidi = matchids[i][j]; - unsigned int matchesi = matchidi.size(); + std::vector & matchidi = matchids[i][j]; + unsigned long matchesi = matchidi.size(); Eigen::Matrix & tXj = transformed_points[j]; Eigen::Matrix & tXnj = transformed_normals[j]; Eigen::VectorXd & informationj = informations[j]; @@ -972,8 +972,8 @@ Eigen::MatrixXd MassRegistrationPPR2::getAllResiduals(std::vector= Qp.cols() || kj < 0 || kj >= tXj.cols() ){continue;} Qp.col(ki) = tXj.col(kj); Qn.col(ki) = tXnj.col(kj); @@ -986,7 +986,7 @@ Eigen::MatrixXd MassRegistrationPPR2::getAllResiduals(std::vector poses){ - unsigned int nr_frames = poses.size(); + unsigned long nr_frames = poses.size(); Eigen::MatrixXd all_residuals; - int total_matches = 0; - for(unsigned int i = 0; i < nr_frames; i++){ + long total_matches = 0; + for(unsigned long i = 0; i < nr_frames; i++){ if(!is_ok[i]){continue;} - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} total_matches += depthedge_matchids[i][j].size(); } @@ -1025,23 +1025,23 @@ Eigen::MatrixXd MassRegistrationPPR2::depthedge_getAllResiduals(std::vector & matchidi = depthedge_matchids[i][j]; - unsigned int matchesi = matchidi.size(); + std::vector & matchidi = depthedge_matchids[i][j]; + unsigned long matchesi = matchidi.size(); Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); @@ -1049,8 +1049,8 @@ Eigen::MatrixXd MassRegistrationPPR2::depthedge_getAllResiduals(std::vector poses){ - unsigned int nr_frames = poses.size(); + unsigned long nr_frames = poses.size(); // printf("is_ok size: %i\n",is_ok.size()); - int total_matches = 0; - for(unsigned int i = 0; i < nr_frames; i++){ - const unsigned int src_nr_ap = kp_nr_arraypoints[i]; + long total_matches = 0; + for(unsigned long i = 0; i < nr_frames; i++){ + const unsigned long src_nr_ap = kp_nr_arraypoints[i]; if(src_nr_ap == 0){continue;} - for(unsigned int j = 0; j < nr_frames; j++){ - const unsigned int dst_nr_ap = kp_nr_arraypoints[j]; + for(unsigned long j = 0; j < nr_frames; j++){ + const unsigned long dst_nr_ap = kp_nr_arraypoints[j]; if(dst_nr_ap == 0){continue;} total_matches += kp_matches[i][j].size(); } @@ -1101,8 +1101,8 @@ Eigen::MatrixXd MassRegistrationPPR2::getAllKpResiduals(std::vector & matches = kp_matches[i][j]; - unsigned int nr_matches = matches.size(); + unsigned long nr_matches = matches.size(); - for(unsigned int k = 0; k < nr_matches; ++k) { + for(unsigned long k = 0; k < nr_matches; ++k) { TestMatch & tm = matches[k]; - unsigned int src_k = tm.src; - unsigned int dst_k = tm.dst; + unsigned long src_k = tm.src; + unsigned long dst_k = tm.dst; const double & src_x = src_ap[3*src_k+0]; const double & src_y = src_ap[3*src_k+1]; @@ -1363,7 +1363,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ informations.push_back( Eigen::VectorXd()); nr_matches.push_back( 0); - matchids.push_back( std::vector< std::vector >() ); + matchids.push_back( std::vector< std::vector >() ); matchdists.push_back( std::vector< std::vector >() ); nr_arraypoints.push_back(0); @@ -1375,7 +1375,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ a3dv.push_back(0); depthedge_nr_matches.push_back( 0); - depthedge_matchids.push_back( std::vector< std::vector >() ); + depthedge_matchids.push_back( std::vector< std::vector >() ); depthedge_matchdists.push_back( std::vector< std::vector >() ); //depthedge_neighbours.push_back(0); @@ -1388,7 +1388,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ is_ok.push_back(false); - unsigned int i = points.size()-1; + unsigned long i = points.size()-1; bool * maskvec = mmask->maskvec; unsigned char * edgedata = (unsigned char *)(frame->depthedges.data); @@ -1397,18 +1397,18 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ float * normalsdata = (float *)(frame->normals.data); Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; + const unsigned long width = camera->width; + const unsigned long height = camera->height; const float idepth = camera->idepth_scale; const float cx = camera->cx; const float cy = camera->cy; const float ifx = 1.0/camera->fx; const float ify = 1.0/camera->fy; - int count = 0; - for(unsigned int w = 0; w < width; w+=maskstep){ - for(unsigned int h = 0; h < height; h+=maskstep){ - int ind = h*width+w; + long count = 0; + for(unsigned long w = 0; w < width; w+=maskstep){ + for(unsigned long h = 0; h < height; h+=maskstep){ + long ind = h*width+w; if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ float z = idepth*float(depthdata[ind]); float xn = normalsdata[3*ind+0]; @@ -1451,11 +1451,11 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ Eigen::Matrix & tXn = transformed_normals[i]; Eigen::VectorXd information (count); - int c = 0; - for(unsigned int w = 0; w < width; w+=maskstep){ - for(unsigned int h = 0; h < height;h+=maskstep){ + long c = 0; + for(unsigned long w = 0; w < width; w+=maskstep){ + for(unsigned long h = 0; h < height;h+=maskstep){ if(c == count){continue;} - int ind = h*width+w; + long ind = h*width+w; //if(maskvec[ind] || nomask){ if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ float z = idepth*float(depthdata[ind]); @@ -1508,10 +1508,10 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ trees3d[i]->buildIndex(); - int depthedge_count = 0; - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height; h++){ - int ind = h*width+w; + long depthedge_count = 0; + for(unsigned long w = 0; w < width; w++){ + for(unsigned long h = 0; h < height; h++){ + long ind = h*width+w; if(maskvec[ind] && edgedata[ind] == 255){ float z = idepth*float(depthdata[ind]); if(z > 0.2){depthedge_count++;} @@ -1523,17 +1523,17 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ if(depthedge_count > 10){ double * depthedge_ap = new double[3*depthedge_count]; double * depthedge_ai = new double[3*depthedge_count]; - //int * dn = new int[depthedge_nr_neighbours*depthedge_count]; + //long * dn = new long[depthedge_nr_neighbours*depthedge_count]; //depthedge_neighbours[i] = dn; depthedge_nr_arraypoints[i] = depthedge_count; depthedge_arraypoints[i] = depthedge_ap; depthedge_arrayinformations[i] = depthedge_ai; c = 0; - for(unsigned int w = 0; w < width; w+=1){ - for(unsigned int h = 0; h < height; h+=1){ + for(unsigned long w = 0; w < width; w+=1){ + for(unsigned long h = 0; h < height; h+=1){ if(c == depthedge_count){continue;} - int ind = h*width+w; + long ind = h*width+w; if(maskvec[ind] && edgedata[ind] == 255){ float z = idepth*float(depthdata[ind]); if(z > 0.2){ @@ -1556,19 +1556,19 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ Tree3d * t3d = depthedge_trees3d[i]; -// const int nrdn = depthedge_nr_neighbours+1; +// const long nrdn = depthedge_nr_neighbours+1; // //#if defdomultithread // #pragma omp parallel for num_threads(8) //#endif -// for(int c = 0; c < depthedge_count; c++){ +// for(long c = 0; c < depthedge_count; c++){ // size_t ret_index[nrdn]; // double out_dist_sqr[nrdn]; // nanoflann::KNNResultSet resultSet(nrdn); // resultSet.init(ret_index, out_dist_sqr ); // t3d->findNeighbors(resultSet, depthedge_ap+3*c, nanoflann::SearchParams(10)); -// for(int k = 0; k < depthedge_nr_neighbours; k++){ +// for(long k = 0; k < depthedge_nr_neighbours; k++){ // dn[depthedge_nr_neighbours*c + k] = ret_index[k+1]; // } // } @@ -1581,7 +1581,7 @@ void MassRegistrationPPR2::addData(pcl::PointCloud::Ptr double total_load_time_start = getTime(); nr_matches.push_back( 0); - matchids.push_back( std::vector< std::vector >() ); + matchids.push_back( std::vector< std::vector >() ); matchdists.push_back( std::vector< std::vector >() ); nr_datas.push_back( 0); @@ -1604,10 +1604,10 @@ void MassRegistrationPPR2::addData(pcl::PointCloud::Ptr a3dv.push_back(0); is_ok.push_back(false); - unsigned int i = points.size()-1; + unsigned long i = points.size()-1; - int count = 0; - for(unsigned int j = 0; j < cloud->points.size(); j+=nomaskstep*nomaskstep){ + long count = 0; + for(unsigned long j = 0; j < cloud->points.size(); j+=nomaskstep*nomaskstep){ if (isValidPoint(cloud->points[j])){count++;} } @@ -1647,8 +1647,8 @@ void MassRegistrationPPR2::addData(pcl::PointCloud::Ptr Eigen::VectorXd information (count); - int c = 0; - for(unsigned int j = 0; j < cloud->points.size(); j+=nomaskstep*nomaskstep){ + long c = 0; + for(unsigned long j = 0; j < cloud->points.size(); j+=nomaskstep*nomaskstep){ pcl::PointXYZRGBNormal p = cloud->points[j]; if (isValidPoint(p)){ @@ -1714,45 +1714,45 @@ void MassRegistrationPPR2::addData(pcl::PointCloud::Ptr std::vector MassRegistrationPPR2::optimize(std::vector poses){ bool onetoone = true; - unsigned int nr_frames = poses.size(); + unsigned long nr_frames = poses.size(); Eigen::MatrixXd Xo1; - int optt = 2; - for(int outer=0; outer < 60; ++outer) { + long optt = 2; + for(long outer=0; outer < 60; ++outer) { if(getTime()-total_time_start > timeout){break;} //printf("outer: %i\n",outer); std::vector poses2 = poses; if(type == PointToPlane && optt == 2){ - for(unsigned int i = 0; i < nr_frames; i++){ + for(unsigned long i = 0; i < nr_frames; i++){ if(getTime()-total_time_start > timeout){break;} if(!is_ok[i]){continue;} - unsigned int count = 0; + unsigned long count = 0; double * api = arraypoints[i]; double * ani = arraynormals[i]; double * aci = arraycolors[i]; double * aii = arrayinformations[i]; - const unsigned int nr_api = nr_arraypoints[i]; + const unsigned long nr_api = nr_arraypoints[i]; - unsigned int kp_count = 0; + unsigned long kp_count = 0; // double * kp_api = kp_arraypoints[i]; // double * kp_ani = kp_arraynormals[i]; // double * kp_aii = kp_arrayinformations[i]; -// const unsigned int kp_nr_api = kp_nr_arraypoints[i]; +// const unsigned long kp_nr_api = kp_nr_arraypoints[i]; - unsigned int depthedge_count = 0; + unsigned long depthedge_count = 0; double * depthedge_api = depthedge_arraypoints[i]; double * depthedge_aii = depthedge_arrayinformations[i]; - const unsigned int depthedge_nr_api = depthedge_nr_arraypoints[i]; + const unsigned long depthedge_nr_api = depthedge_nr_arraypoints[i]; Eigen::Affine3d rpi = Eigen::Affine3d(poses[i]); const double & mi00 = rpi(0,0); const double & mi01 = rpi(0,1); const double & mi02 = rpi(0,2); const double & mi03 = rpi(0,3); const double & mi10 = rpi(1,0); const double & mi11 = rpi(1,1); const double & mi12 = rpi(1,2); const double & mi13 = rpi(1,3); const double & mi20 = rpi(2,0); const double & mi21 = rpi(2,1); const double & mi22 = rpi(2,2); const double & mi23 = rpi(2,3); - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} if(i == j){continue;} @@ -1760,16 +1760,16 @@ std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector & matchidj = matchids[j][i]; - unsigned int matchesj = matchidj.size(); - std::vector & matchidi = matchids[i][j]; - unsigned int matchesi = matchidi.size(); + std::vector & matchidj = matchids[j][i]; + unsigned long matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned long matchesi = matchidi.size(); std::vector & matchdistj = matchdists[j][i]; std::vector & matchdisti = matchdists[i][j]; - for(unsigned int ki = 0; true && ki < matchesi; ki++){ - int kj = matchidi[ki]; + for(unsigned long ki = 0; true && ki < matchesi; ki++){ + long kj = matchidi[ki]; if( kj == -1 ){continue;} if( kj >= matchesj){continue;} if(!onetoone || matchidj[kj] == ki){ @@ -1828,17 +1828,17 @@ std::vector MassRegistrationPPR2::optimize(std::vector & matchidj = depthedge_matchids[j][i]; - unsigned int matchesj = matchidj.size(); - std::vector & matchidi = depthedge_matchids[i][j]; - unsigned int matchesi = matchidi.size(); + std::vector & matchidj = depthedge_matchids[j][i]; + unsigned long matchesj = matchidj.size(); + std::vector & matchidi = depthedge_matchids[i][j]; + unsigned long matchesi = matchidi.size(); std::vector & matchdistj = depthedge_matchdists[j][i]; std::vector & matchdisti = depthedge_matchdists[i][j]; - for(unsigned int ki = 0; true && ki < matchesi; ki++){ - int kj = matchidi[ki]; + for(unsigned long ki = 0; true && ki < matchesi; ki++){ + long kj = matchidi[ki]; if( kj == -1 ){continue;} if( kj >= matchesj){continue;} if(!onetoone || matchidj[kj] == ki){ @@ -1867,11 +1867,11 @@ std::vector MassRegistrationPPR2::optimize(std::vector 0 && kp_matches[i].size() > 0 && kp_matches[i][j].size() > 0){ std::vector< TestMatch > & matches = kp_matches[i][j]; - unsigned int nr_matches = matches.size(); - for(unsigned int k = 0; k < nr_matches; k++){ + unsigned long nr_matches = matches.size(); + for(unsigned long k = 0; k < nr_matches; k++){ TestMatch & m = matches[k]; - int ki = m.src; - int kj = m.dst; + long ki = m.src; + long kj = m.dst; kp_Xp_arr[3*kp_count+0] = kp_api[3*ki+0]; kp_Xp_arr[3*kp_count+1] = kp_api[3*ki+1]; @@ -1917,13 +1917,13 @@ std::vector MassRegistrationPPR2::optimize(std::vector Xv; - //if(visualizationLvl == 4){for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + //if(visualizationLvl == 4){for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} Eigen::Affine3d p = rpi; bool do_inner = true; - for(int inner=0; inner < 15 && do_inner; ++inner) { + for(long inner=0; inner < 15 && do_inner; ++inner) { pcl::PointCloud::Ptr scloud (new pcl::PointCloud); pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); @@ -1944,7 +1944,7 @@ std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector timeout){break;} if(!is_ok[i]){continue;} - unsigned int count = 0; + unsigned long count = 0; Eigen::Matrix & tXi = transformed_points[i]; Eigen::Matrix & Xi = points[i]; @@ -2356,23 +2356,23 @@ std::vector MassRegistrationPPR2::optimize(std::vector & tXj = transformed_points[j]; Eigen::Matrix & tXnj = transformed_normals[j]; Eigen::VectorXd & informationj = informations[j]; - std::vector & matchidj = matchids[j][i]; - unsigned int matchesj = matchidj.size(); - std::vector & matchidi = matchids[i][j]; - unsigned int matchesi = matchidi.size(); + std::vector & matchidj = matchids[j][i]; + unsigned long matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned long matchesi = matchidi.size(); std::vector & matchdistj = matchdists[j][i]; std::vector & matchdisti = matchdists[i][j]; - for(unsigned int ki = 0; ki < matchesi; ki++){ - int kj = matchidi[ki]; + for(unsigned long ki = 0; ki < matchesi; ki++){ + long kj = matchidi[ki]; if( kj == -1 ){continue;} if( kj >= matchesj){continue;} if(!onetoone || matchidj[kj] == ki){ @@ -2407,13 +2407,13 @@ std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector timeout){break;} if(!is_ok[i]){continue;} - unsigned int nr_match = 0; + unsigned long nr_match = 0; { - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} - std::vector & matchidj = matchids[j][i]; - unsigned int matchesj = matchidj.size(); - std::vector & matchidi = matchids[i][j]; - unsigned int matchesi = matchidi.size(); + std::vector & matchidj = matchids[j][i]; + unsigned long matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned long matchesi = matchidi.size(); std::vector & matchdistj = matchdists[j][i]; std::vector & matchdisti = matchdists[i][j]; - for(unsigned int ki = 0; ki < matchesi; ki++){ - int kj = matchidi[ki]; + for(unsigned long ki = 0; ki < matchesi; ki++){ + long kj = matchidi[ki]; if( kj == -1 ){continue;} if( kj >= matchesj){continue;} if(!onetoone || matchidj[kj] == ki){ nr_match++;} @@ -2554,7 +2554,7 @@ std::vector MassRegistrationPPR2::optimize(std::vector & tXi = transformed_points[i]; Eigen::Matrix & Xi = points[i]; @@ -2562,23 +2562,23 @@ std::vector MassRegistrationPPR2::optimize(std::vector & Xni = normals[i]; Eigen::VectorXd & informationi = informations[i]; - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} Eigen::Matrix & tXj = transformed_points[j]; Eigen::Matrix & tXnj = transformed_normals[j]; Eigen::VectorXd & informationj = informations[j]; - std::vector & matchidj = matchids[j][i]; - unsigned int matchesj = matchidj.size(); - std::vector & matchidi = matchids[i][j]; - unsigned int matchesi = matchidi.size(); + std::vector & matchidj = matchids[j][i]; + unsigned long matchesj = matchidj.size(); + std::vector & matchidi = matchids[i][j]; + unsigned long matchesi = matchidi.size(); std::vector & matchdistj = matchdists[j][i]; std::vector & matchdisti = matchdists[i][j]; - for(unsigned int ki = 0; ki < matchesi; ki++){ - int kj = matchidi[ki]; + for(unsigned long ki = 0; ki < matchesi; ki++){ + long kj = matchidi[ki]; if( kj == -1 ){continue;} if( kj >= matchesj){continue;} if(!onetoone || matchidj[kj] == ki){ @@ -2598,13 +2598,13 @@ std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vectorgetProbs(residuals); } break; case PointToPlane: { W = func->getProbs(residuals); - for(int k=0; k 0.0);} + for(long k=0; k 0.0);} } break; default: {printf("type not set\n");} break; } @@ -2635,7 +2635,7 @@ std::vector MassRegistrationPPR2::optimize(std::vector(); Xp = rot*Xp; Xn = rot.rotation()*Xn; @@ -2652,13 +2652,13 @@ std::vector MassRegistrationPPR2::optimize(std::vector().matrix(); } } Eigen::Matrix4d p0inv = poses[0].inverse(); - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} poses[j] = p0inv*poses[j]; @@ -2672,7 +2672,7 @@ std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector poses, std::vector prev_poses, bool first){ printf("rematchKeyPoints\n"); double total_rematchKeyPoints_time_start = getTime(); - unsigned int nr_frames = poses.size(); + unsigned long nr_frames = poses.size(); - int max_kps = 0; - for(unsigned int i = 0; i < nr_frames; i++){ + long max_kps = 0; + for(unsigned long i = 0; i < nr_frames; i++){ printf("max_kps: %i kp_nr_arraypoints: %i\n",max_kps, kp_nr_arraypoints[i]); max_kps = std::max(max_kps,kp_nr_arraypoints[i]); } @@ -2765,44 +2765,44 @@ void MassRegistrationPPR2::rematchKeyPoints(std::vector poses, double * best_src2 = new double[max_kps]; double * best_src_e = new double[max_kps]; double * best_src_f = new double[max_kps]; - int * best_src_id = new int[max_kps]; + long * best_src_id = new long[max_kps]; double * best_dst = new double[max_kps]; double * best_dst_e = new double[max_kps]; double * best_dst_f = new double[max_kps]; - int * best_dst_id = new int[max_kps]; + long * best_dst_id = new long[max_kps]; - for(unsigned int i = 0; i < nr_frames; i++){ + for(unsigned long i = 0; i < nr_frames; i++){ kp_matches.resize(nr_frames); - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ kp_matches[i].resize(nr_frames); } } - int ignores = 0; + long ignores = 0; bool useOrb = true; if(useOrb){ - for(unsigned int i = 0; i < nr_frames; i++){ - const unsigned int src_nr_ap = kp_nr_arraypoints[i]; + for(unsigned long i = 0; i < nr_frames; i++){ + const unsigned long src_nr_ap = kp_nr_arraypoints[i]; if(src_nr_ap == 0){continue;} double * src_ap = kp_arraypoints[i]; double * src_an = kp_arraynormals[i]; double * src_ai = kp_arrayinformations[i]; uint64_t * src_data = kp_arraydescriptors[i]; - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(i == j){continue;} - const unsigned int dst_nr_ap = kp_nr_arraypoints[j]; + const unsigned long dst_nr_ap = kp_nr_arraypoints[j]; if(dst_nr_ap == 0){continue;} - for(int k = 0; k < src_nr_ap; k++){ + for(long k = 0; k < src_nr_ap; k++){ best_src[k] = 999999999999; best_src2[k] = 999999999999; best_src_id[k] = -1; } - for(int k = 0; k < dst_nr_ap; k++){ + for(long k = 0; k < dst_nr_ap; k++){ best_dst[k] = 999999999999; best_dst_id[k] = -1; } @@ -2823,9 +2823,9 @@ if(useOrb){ double change_trans = 0; double change_rot = 0; double dt = 0; - for(unsigned int k = 0; k < 3; k++){ + for(unsigned long k = 0; k < 3; k++){ dt += diff(k,3)*diff(k,3); - for(unsigned int l = 0; l < 3; l++){ + for(unsigned long l = 0; l < 3; l++){ if(k == l){ change_rot += fabs(1-diff(k,l));} else{ change_rot += fabs(diff(k,l));} } @@ -2847,7 +2847,7 @@ if(useOrb){ #if defdomultithread #pragma omp parallel for num_threads(8) #endif - for(unsigned int src_k = 0; src_k < src_nr_ap; ++src_k) { + for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { const double & src_x = src_ap[3*src_k+0]; const double & src_y = src_ap[3*src_k+1]; const double & src_z = src_ap[3*src_k+2]; @@ -2856,21 +2856,21 @@ if(useOrb){ double sy = m10*src_x + m11*src_y + m12*src_z + m13; double sz = m20*src_x + m21*src_y + m22*src_z + m23; - unsigned int src_k4 = src_k*4; + unsigned long src_k4 = src_k*4; const uint64_t s1 = src_data[src_k4+0]; const uint64_t s2 = src_data[src_k4+1]; const uint64_t s3 = src_data[src_k4+2]; const uint64_t s4 = src_data[src_k4+3]; uint64_t xordata [4]; - for(unsigned int dst_k = 0; dst_k < dst_nr_ap; ++dst_k) { + for(unsigned long dst_k = 0; dst_k < dst_nr_ap; ++dst_k) { double dx = sx-dst_ap[3*dst_k+0]; double dy = sy-dst_ap[3*dst_k+1]; double dz = sz-dst_ap[3*dst_k+2]; double p_dist = sqrt(dx*dx+dy*dy+dz*dz); - unsigned int dst_k4 = dst_k*4; + unsigned long dst_k4 = dst_k*4; const uint64_t d1 = dst_data[dst_k4+0]; const uint64_t d2 = dst_data[dst_k4+1]; const uint64_t d3 = dst_data[dst_k4+2]; @@ -2881,7 +2881,7 @@ if(useOrb){ xordata[2] = s3 ^ d3; xordata[3] = s4 ^ d4; - int cnt = popcount_lauradoux(xordata, 4); + long cnt = popcount_lauradoux(xordata, 4); double f_dist = float(cnt)/256.0f; float d = f_dist*di + p_dist*pi; @@ -2909,9 +2909,9 @@ if(useOrb){ double threshold = 2.0; double ratiothreshold = 0.8; - int nr_matches = 0; - for(unsigned int src_k = 0; src_k < src_nr_ap; ++src_k) { - int dst_k = best_src_id[src_k]; + long nr_matches = 0; + for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { + long dst_k = best_src_id[src_k]; if(best_dst_id[dst_k] != src_k){continue;}//One to one if(best_src[src_k] > threshold){continue;}//threshold @@ -2931,25 +2931,25 @@ if(useOrb){ bool useSurf = false; if(useSurf){ - for(unsigned int i = 0; i < nr_frames; i++){ - const unsigned int src_nr_ap = kp_nr_arraypoints[i]; + for(unsigned long i = 0; i < nr_frames; i++){ + const unsigned long src_nr_ap = kp_nr_arraypoints[i]; if(src_nr_ap == 0){continue;} double * src_ap = kp_arraypoints[i]; double * src_an = kp_arraynormals[i]; double * src_ai = kp_arrayinformations[i]; float* src_data = (float*)(kp_arraydescriptors[i]); - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(i == j){continue;} - const unsigned int dst_nr_ap = kp_nr_arraypoints[j]; + const unsigned long dst_nr_ap = kp_nr_arraypoints[j]; if(dst_nr_ap == 0){continue;} - for(int k = 0; k < src_nr_ap; k++){ + for(long k = 0; k < src_nr_ap; k++){ best_src[k] = 999999999999; best_src_id[k] = -1; } - for(int k = 0; k < dst_nr_ap; k++){ + for(long k = 0; k < dst_nr_ap; k++){ best_dst[k] = 999999999999; best_dst_id[k] = -1; } @@ -2970,9 +2970,9 @@ if(useSurf){ double change_trans = 0; double change_rot = 0; double dt = 0; - for(unsigned int k = 0; k < 3; k++){ + for(unsigned long k = 0; k < 3; k++){ dt += diff(k,3)*diff(k,3); - for(unsigned int l = 0; l < 3; l++){ + for(unsigned long l = 0; l < 3; l++){ if(k == l){ change_rot += fabs(1-diff(k,l));} else{ change_rot += fabs(diff(k,l));} } @@ -2991,7 +2991,7 @@ if(useSurf){ #if defdomultithread #pragma omp parallel for num_threads(8) #endif - for(unsigned int src_k = 0; src_k < src_nr_ap; ++src_k) { + for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { const double & src_x = src_ap[3*src_k+0]; const double & src_y = src_ap[3*src_k+1]; const double & src_z = src_ap[3*src_k+2]; @@ -3002,7 +3002,7 @@ if(useSurf){ - for(unsigned int dst_k = 0; dst_k < dst_nr_ap; ++dst_k) { + for(unsigned long dst_k = 0; dst_k < dst_nr_ap; ++dst_k) { double dx = sx-dst_ap[3*dst_k+0]; double dy = sy-dst_ap[3*dst_k+1]; double dz = sz-dst_ap[3*dst_k+2]; @@ -3010,7 +3010,7 @@ if(useSurf){ double f_dist = 0; - for(unsigned int f_k = 0; f_k < 128; ++f_k) { + for(unsigned long f_k = 0; f_k < 128; ++f_k) { double diff = src_data[src_k*128 + f_k] - dst_data[dst_k*128 + f_k]; f_dist += diff*diff; } @@ -3032,9 +3032,9 @@ if(useSurf){ std::vector< TestMatch > matches; double threshold = 0.25; - int nr_matches = 0; - for(unsigned int src_k = 0; src_k < src_nr_ap; ++src_k) { - int dst_k = best_src_id[src_k]; + long nr_matches = 0; + for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { + long dst_k = best_src_id[src_k]; if(best_dst_id[dst_k] != src_k || best_src[src_k] > threshold){continue;}//One to one @@ -3049,9 +3049,9 @@ if(useSurf){ } } if(false){ - for(unsigned int i = 0; i < nr_frames; i++){ + for(unsigned long i = 0; i < nr_frames; i++){ if(frameid[i] == -1){continue;} - for(unsigned int j = 0; j < nr_frames; j++){ + for(unsigned long j = 0; j < nr_frames; j++){ if(frameid[j] == -1){continue;} if(kp_matches[i][j].size() == 0){continue;} unsigned char * src_data = (unsigned char *)(model->frames[frameid[i]]->rgb.data); @@ -3060,12 +3060,12 @@ if(false){ cv::Mat img; img.create(480,2*640,CV_8UC3); unsigned char * imgdata = (unsigned char *)img.data; - for(int w = 0; w < 640; w++){ - for(int h = 0; h < 480; h++){ - int ind = h*640+w; + for(long w = 0; w < 640; w++){ + for(long h = 0; h < 480; h++){ + long ind = h*640+w; - int ind2 = h*2*640+w; - int ind3 = h*2*640+w+640; + long ind2 = h*2*640+w; + long ind3 = h*2*640+w+640; imgdata[3*ind2+0] = src_data[3*ind+0]; imgdata[3*ind2+1] = src_data[3*ind+1]; @@ -3081,7 +3081,7 @@ if(false){ std::vector & dst_keypoints = model->all_keypoints[j]; std::vector< TestMatch > matches = kp_matches[i][j]; - for(unsigned int k = 0; k < matches.size(); ++k) { + for(unsigned long k = 0; k < matches.size(); ++k) { cv::KeyPoint & spt = src_keypoints[matches[k].src]; cv::KeyPoint & dpt = dst_keypoints[matches[k].dst]; @@ -3093,7 +3093,7 @@ if(false){ cv::imshow( "matches", img ); cv::waitKey(0); - //for(int j = 0; j < width*height; j++){maskdata[j] = 255*maskvec[j];} + //for(long j = 0; j < width*height; j++){maskdata[j] = 255*maskvec[j];} } } } @@ -3114,19 +3114,19 @@ if(false){ void MassRegistrationPPR2::showEdges(std::vector poses){ printf("showEdges\n"); pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - for(unsigned int i = 0; i < poses.size(); i++){ + for(unsigned long i = 0; i < poses.size(); i++){ printf("%i/%i -> %i\n",i,poses.size(),depthedge_nr_arraypoints[i]); Eigen::Matrix4d p = poses[i]; float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - int r,g,b; + long r,g,b; r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); g = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1); - for(unsigned int c = 0; c < depthedge_nr_arraypoints[i]; c++){ + for(unsigned long c = 0; c < depthedge_nr_arraypoints[i]; c++){ pcl::PointXYZRGBNormal p; float x = depthedge_arraypoints.at(i)[3*c+0]; @@ -3153,7 +3153,7 @@ void MassRegistrationPPR2::showEdges(std::vector poses){ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses){ if(visualizationLvl > 0){printf("start MassRegistrationPPR2::getTransforms(std::vector poses)\n");} - unsigned int nr_frames = informations.size(); + unsigned long nr_frames = informations.size(); if(poses.size() != nr_frames){ printf("ERROR: poses.size() = %i != informations.size() %i\n",poses.size(), informations.size()); return MassFusionResults(); @@ -3167,9 +3167,9 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector & Xn = normals[i]; Eigen::Matrix & tX = transformed_points[i]; Eigen::Matrix & tXn = transformed_normals[i]; - int count = nr_datas[i]; - for(int c = 0; c < count; c++){ + long count = nr_datas[i]; + for(long c = 0; c < count; c++){ float x = X(0,c); float y = X(1,c); float z = X(2,c); @@ -3213,13 +3213,13 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectorreset(); depthedge_func->reset(); - int imgcount = 0; + long imgcount = 0; char buf [1024]; if(visualizationLvl == 1){ if(use_depthedge){showEdges(poses);} if(use_surface){ std::vector Xv; - for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} sprintf(buf,"image%5.5i.png",imgcount++); show(Xv,false,std::string(buf),imgcount); } @@ -3241,26 +3241,26 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses0 = poses; //rematchKeyPoints(poses,poses0,first); - for(int funcupdate=0; funcupdate < 100; ++funcupdate) { + for(long funcupdate=0; funcupdate < 100; ++funcupdate) { if(getTime()-total_time_start > timeout){break;} - //if(visualizationLvl == 2){if(use_depthedge){showEdges(poses);}std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount); showEdges(poses);} + //if(visualizationLvl == 2){if(use_depthedge){showEdges(poses);}std::vector Xv;for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount); showEdges(poses);} if(visualizationLvl == 2){ if(use_depthedge){showEdges(poses);} if(use_surface){ std::vector Xv; - for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} sprintf(buf,"image%5.5i.png",imgcount++); show(Xv,false,std::string(buf),imgcount); } } - for(int rematching=0; rematching < 10; ++rematching) { - //if(visualizationLvl == 3){if(use_depthedge){showEdges(poses);}std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + for(long rematching=0; rematching < 10; ++rematching) { + //if(visualizationLvl == 3){if(use_depthedge){showEdges(poses);}std::vector Xv;for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} if(visualizationLvl == 3){ if(use_depthedge){showEdges(poses);} if(use_surface){ std::vector Xv; - for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} sprintf(buf,"image%5.5i.png",imgcount++); show(Xv,false,std::string(buf),imgcount); } @@ -3275,7 +3275,7 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector 0){ printf("funcupdate: %i rematching: %i lala: %i\n",funcupdate,rematching,lala); printf("total_time: %5.5f\n",getTime()-total_time_start); @@ -3284,12 +3284,12 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector Xv;for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} + //if(visualizationLvl == 4){if(use_depthedge){showEdges(poses);}std::vector Xv;for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount);} if(visualizationLvl == 4){ if(use_depthedge){showEdges(poses);} if(use_surface){ std::vector Xv; - for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} sprintf(buf,"image%5.5i.png",imgcount++); show(Xv,false,std::string(buf),imgcount); } @@ -3390,13 +3390,13 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector Xv; - for(unsigned int j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} sprintf(buf,"image%5.5i.png",imgcount++); show(Xv,false,std::string(buf),imgcount); } Eigen::Matrix4d firstinv = poses.front().inverse(); - for(int i = 0; i < nr_frames; i++){poses[i] = firstinv*poses[i];} + for(long i = 0; i < nr_frames; i++){poses[i] = firstinv*poses[i];} printf("stop MassRegistrationPPR2::getTransforms(std::vector guess)\n"); //exit(0); diff --git a/quasimodo/quasimodo_retrieval/CMakeLists.txt b/quasimodo/quasimodo_retrieval/CMakeLists.txt index e1b93d01..a00e5bf4 100644 --- a/quasimodo/quasimodo_retrieval/CMakeLists.txt +++ b/quasimodo/quasimodo_retrieval/CMakeLists.txt @@ -8,7 +8,7 @@ add_definitions(-std=c++11 -O3) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp tf sensor_msgs std_msgs image_geometry metaroom_xml_parser quasimodo_msgs message_runtime message_generation tf_conversions pcl_ros image_geometry cv_bridge tf_conversions eigen_conversions - k_means_tree dynamic_object_retrieval object_3d_benchmark dynamic_reconfigure soma2_msgs) + k_means_tree dynamic_object_retrieval object_3d_benchmark dynamic_reconfigure soma_msgs) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -185,7 +185,7 @@ add_dependencies(quasimodo_visualization_server quasimodo_msgs_generate_messages add_dependencies(quasimodo_visualize_model quasimodo_msgs_generate_messages_cpp) add_dependencies(quasimodo_retrieval_node quasimodo_msgs_generate_messages_cpp) add_dependencies(quasimodo_retrieve_observation quasimodo_msgs_generate_messages_cpp) -add_dependencies(quasimodo_logging_server quasimodo_msgs_generate_messages_cpp soma2_msgs_generate_messages_cpp) +add_dependencies(quasimodo_logging_server quasimodo_msgs_generate_messages_cpp soma_msgs_generate_messages_cpp) ## Add cmake target dependencies of the executable ## same as for the library above diff --git a/quasimodo/quasimodo_retrieval/package.xml b/quasimodo/quasimodo_retrieval/package.xml index 967233e0..9b5915f1 100644 --- a/quasimodo/quasimodo_retrieval/package.xml +++ b/quasimodo/quasimodo_retrieval/package.xml @@ -58,7 +58,7 @@ dynamic_reconfigure libqt4-dev libqt4-opengl-dev - soma2_msgs + soma_msgs libpcl-all libopencv-dev cv_bridge @@ -77,7 +77,7 @@ dynamic_reconfigure libqt4 libqt4-opengl - soma2_msgs + soma_msgs From 9e9a9a5c1d4f270f79f261232d81e1242e3e477b Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 26 Aug 2016 16:24:34 +0200 Subject: [PATCH 077/255] integration between segmentation and modelserver --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 3 +- quasimodo/quasimodo_brain/src/Util/Util.h | 1 + .../metaroom_additional_view_processing.cpp | 372 +- quasimodo/quasimodo_brain/src/modelserver.cpp | 332 +- .../quasimodo_models/include/core/RGBDFrame.h | 1 + .../quasimodo_models/src/core/RGBDFrame.cpp | 4 + .../src/modelupdater/ModelUpdater.cpp | 7212 ++++++++--------- 7 files changed, 3979 insertions(+), 3946 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 5cc02df1..03ddb421 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -157,6 +157,7 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ std::vector current_room_frames; for (size_t i=0; i 8){continue;} cv::Mat fullmask; fullmask.create(480,640,CV_8UC1); @@ -199,7 +200,7 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, m); current_room_frames.push_back(frame); - if(i == 0){ + if(sweepmodel == 0){ sweepmodel = new reglib::Model(frame,fullmask); }else{ sweepmodel->frames.push_back(frame); diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index b7386fd9..f831ac30 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -46,6 +46,7 @@ Eigen::Matrix4d getMat(tf::StampedTransform tf); reglib::Model * load_metaroom_model(std::string sweep_xml); void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg = false); +std::vector loadModelsXML(std::string path); } diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 89366f57..f3c19a8f 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -79,17 +79,46 @@ std::string overall_folder = "~/.semanticMap/"; boost::shared_ptr viewer; int visualization_lvl = 0; std::string outtopic = "/some/topic"; + +std::string modelouttopic = "/model/out/topic"; ros::Publisher out_pub; -std::string posepath = ""; +ros::Publisher model_pub; +std::string posepath = "testposes.xml"; std::vector sweepPoses; +#include +#include + void remove_old_seg(std::string sweep_folder){ - char buf [1024]; - sprintf(buf,"rm %s/dynamic_*.png",sweep_folder.c_str()); - printf("%s\n",buf); + DIR *dir; + struct dirent *ent; + if ((dir = opendir (sweep_folder.c_str())) != NULL) { + /* print all the files and directories within directory */ + while ((ent = readdir (dir)) != NULL) { + std::string file = std::string(ent->d_name); + + if (file.find("dynamic_object") !=std::string::npos && file.find(".xml") !=std::string::npos){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } - sprintf(buf,"rm %s/moving_*.png",sweep_folder.c_str()); - printf("%s\n",buf); + if (file.find("dynamicmask") !=std::string::npos && file.find(".png") !=std::string::npos){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } + + if (file.find("moving_object") !=std::string::npos && file.find(".xml") !=std::string::npos){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } + + if (file.find("movingmask") !=std::string::npos && file.find(".png") !=std::string::npos){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } + } + closedir (dir); + } } void writePose(QXmlStreamWriter* xmlWriter, Eigen::Matrix4d pose){ @@ -561,6 +590,8 @@ std::vector readPoseXML(std::string xmlFile){ } } delete xmlReader; + printf("done readPoseXML\n"); + //exit(0); return poses; } @@ -572,9 +603,9 @@ void processMetaroom(std::string path){ reglib::Model * fullmodel = processAV(path); - savePoses(sweep_folder+"testposes.xml",fullmodel->relativeposes,17); + //savePoses(sweep_folder+"testposes.xml",fullmodel->relativeposes,17); - writeXml(path+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); + writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( fullmodel, reg); @@ -618,7 +649,7 @@ void processMetaroom(std::string path){ std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; - quasimodo_brain::segment(bg,models,internal,external,dynamic,false); + quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization_lvl > 0); bg->fullDelete(); delete bg; @@ -712,6 +743,12 @@ void processMetaroom(std::string path){ } } + if(visualization_lvl > 0 && cloud->points.size() > 0){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + viewer->spin(); + } + printf("dynamiccloud: %i\n",dynamiccloud->points.size()); if(dynamiccloud->points.size() > 0){ // Creating the KdTree object for the search method of the extraction @@ -840,13 +877,8 @@ void processMetaroom(std::string path){ delete xmlWriter; } } - if(visualization_lvl > 0){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - viewer->spin(); - } - } + } } @@ -893,7 +925,7 @@ void trainMetaroom(std::string path){ bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); - savePoses(posepath,bgmfr.poses,17); + savePoses(overall_folder+"/"+posepath,bgmfr.poses,17); fullmodel->fullDelete(); delete fullmodel; delete bgmassreg; @@ -927,7 +959,6 @@ std::vector loadModels(std::string path){ reglib::Model * mod = new reglib::Model(); QXmlStreamReader* xmlReader = new QXmlStreamReader(&file); -/* while (!xmlReader->atEnd() && !xmlReader->hasError()) { QXmlStreamReader::TokenType token = xmlReader->readNext(); @@ -937,7 +968,7 @@ std::vector loadModels(std::string path){ if (xmlReader->hasError()) { ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); - return; + break; } QString elementName = xmlReader->name().toString(); @@ -945,272 +976,88 @@ std::vector loadModels(std::string path){ if (token == QXmlStreamReader::StartElement) { - if (xmlReader->name() == "View") + if (xmlReader->name() == "Mask") { - cv::Mat rgb; - cv::Mat depth; - //printf("elementName: %s\n",elementName.toStdString().c_str()); + int number = 0; + cv::Mat mask; QXmlStreamAttributes attributes = xmlReader->attributes(); - if (attributes.hasAttribute("RGB")) - { - QString rgbpath = attributes.value("RGB").toString(); - //printf("rgb filename: %s\n",rgbpath.toStdString().c_str()); - rgb = cv::imread(rgbpath.toStdString().c_str(), CV_LOAD_IMAGE_UNCHANGED); + if (attributes.hasAttribute("filename")){ + QString maskpath = attributes.value("filename").toString(); + printf("mask filename: %s\n",maskpath.toStdString().c_str()); + mask = cv::imread(maskpath.toStdString().c_str(), CV_LOAD_IMAGE_UNCHANGED); }else{break;} - if (attributes.hasAttribute("DEPTH")) + if (attributes.hasAttribute("image_number")) { - QString depthpath = attributes.value("DEPTH").toString(); - //printf("depth filename: %s\n",depthpath.toStdString().c_str()); - depth = cv::imread(depthpath.toStdString().c_str(), CV_LOAD_IMAGE_UNCHANGED); - }else{break;} - - - token = xmlReader->readNext();//Stamp - elementName = xmlReader->name().toString(); - - token = xmlReader->readNext();//sec - elementName = xmlReader->name().toString(); - int sec = atoi(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//nsec - elementName = xmlReader->name().toString(); - int nsec = atoi(xmlReader->readElementText().toStdString().c_str()); - token = xmlReader->readNext();//end stamp - - token = xmlReader->readNext();//Camera - elementName = xmlReader->name().toString(); - - reglib::Camera * cam = new reglib::Camera(); - - token = xmlReader->readNext();//fx - cam->fx = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//fy - cam->fy = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//cx - cam->cx = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//cy - cam->cy = atof(xmlReader->readElementText().toStdString().c_str()); + QString depthpath = attributes.value("image_number").toString(); + number = atoi(depthpath.toStdString().c_str()); + printf("number: %i\n",number); - token = xmlReader->readNext();//Camera - elementName = xmlReader->name().toString(); - double time = double(sec)+double(nsec)/double(1e9); - - token = xmlReader->readNext();//RegisteredPose - elementName = xmlReader->name().toString(); - - Eigen::Matrix4d regpose = getPose(xmlReader); - - token = xmlReader->readNext();//RegisteredPose - elementName = xmlReader->name().toString(); - - - token = xmlReader->readNext();//Pose - elementName = xmlReader->name().toString(); - - Eigen::Matrix4d pose = getPose(xmlReader); - - token = xmlReader->readNext();//Pose - elementName = xmlReader->name().toString(); + }else{break;} - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, pose); - frames.push_back(frame); - poses.push_back(pose); + mod->frames.push_back(frames[number]->clone()); + mod->relativeposes.push_back(poses[number]); + mod->modelmasks.push_back(new reglib::ModelMask(mask)); } } } -*/ - delete xmlReader; - -// QXmlStreamWriter* xmlWriter = new QXmlStreamWriter(); -// xmlWriter->setDevice(&file); - -// xmlWriter->writeStartDocument(); -// xmlWriter->writeStartElement("Object"); -// xmlWriter->writeAttribute("object_number", QString::number(d)); - -// for(unsigned int j = 0; j < mod_fr.size(); j++){ -// if(inds[j].size() == 0){continue;} -// reglib::RGBDFrame * frame = mod_fr[j]; -// reglib::Camera * cam = frame->camera; -// cv::Mat mask; -// mask.create(cam->height,cam->width,CV_8UC1); -// unsigned char * maskdata = (unsigned char *)(mask.data); -// for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 0;} -// for(unsigned int k = 0; k < inds[j].size(); k++){maskdata[inds[j][k]] = 255;} - -// char buf [1024]; -// sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),d,j); -// cv::imwrite(buf, mask ); -// xmlWriter->writeStartElement("Mask"); -// xmlWriter->writeAttribute("filename", QString(buf)); -// xmlWriter->writeAttribute("image_number", QString::number(j)); -// xmlWriter->writeEndElement(); -// } - -// xmlWriter->writeEndElement(); -// xmlWriter->writeEndDocument(); -// delete xmlWriter; + delete xmlReader; models.push_back(mod); - } -// std::vector rgbs; -// std::vector depths; - -// for(int imgnr = 0; true; imgnr++){ -// char buf [1024]; - -// sprintf(buf,"%s/view_RGB%10.10i.png",sweep_folder.c_str(),imgnr); -// cv::Mat rgb = cv::imread(buf, CV_LOAD_IMAGE_UNCHANGED); - -// sprintf(buf,"%s/view_DEPTH%10.10i.png",sweep_folder.c_str(),imgnr); -// cv::Mat depth = cv::imread(buf, CV_LOAD_IMAGE_UNCHANGED); -// if(!rgb.data || !depth.data){break;} - -// rgbs.push_back(rgb); -// depths.push_back(depth); - -// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); -// cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", depth); -// cv::waitKey(0); -// } + for(unsigned int i = 0; i < frames.size(); i++){delete frames[i];} return models; -/* +} +void addModelToModelServer(reglib::Model * model){ + model_pub.publish(quasimodo_brain::getModelMSG(model)); + ros::spinOnce(); +} - std::vector viewrgbs; - std::vector viewdepths; - std::vector viewtfs; +void sendMetaroomToServer(std::string path){ + std::vector mods = loadModels(path); + for(unsigned int i = 0; i < mods.size(); i++){ + addModelToModelServer(mods[i]); + mods[i]->fullDelete(); + delete mods[i]; + } +} - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); - for (auto objectFile : objectFiles){ - auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); - for (unsigned int i=0; iheight,cloud->width,CV_8UC3); - unsigned char * rgbdata = (unsigned char *)rgb.data; +void sendCallback(const std_msgs::String::ConstPtr& msg){ + sendMetaroomToServer(msg->data); +} - cv::Mat depth; - depth.create(cloud->height,cloud->width,CV_16UC1); - unsigned short * depthdata = (unsigned short *)depth.data; +int main(int argc, char** argv){ - unsigned int nr_data = cloud->height * cloud->width; - for(unsigned int j = 0; j < nr_data; j++){ - PointType p = cloud->points[j]; - rgbdata[3*j+0] = p.b; - rgbdata[3*j+1] = p.g; - rgbdata[3*j+2] = p.r; - depthdata[j] = short(5000.0 * p.z); + const rlim_t kStackSize = 256 * 1024 * 1024; // min stack size = 256 MB + struct rlimit rl; + unsigned long result; + + result = getrlimit(RLIMIT_STACK, &rl); + //printf("result: %ld mb and %ld mb\n",rl.rlim_cur/(1024*1024),rl.rlim_max/(1024*1024)); + if (result == 0){ + if (rl.rlim_cur < kStackSize){ + rl.rlim_cur = kStackSize; + result = setrlimit(RLIMIT_STACK, &rl); + if (result != 0){ + fprintf(stderr, "setrlimit returned result = %d\n", result); } - - viewrgbs.push_back(rgb); - viewdepths.push_back(depth); - viewtfs.push_back(object.vAdditionalViewsTransforms[i]); + //printf("result: %ld mb and %ld mb\n",rl.rlim_cur/(1024*1024),rl.rlim_max/(1024*1024)); } } - reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path); - for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ - sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; - sweep->relativeposes[i] = sweepPoses[i]; - } - - std::vector frames; - std::vector masks; - std::vector unrefined; - - std::vector both_unrefined; - both_unrefined.push_back(Eigen::Matrix4d::Identity()); - std::vector times; - for(unsigned int i = 0; i < viewrgbs.size(); i++){ - printf("additional view: %i\n",i); - geometry_msgs::TransformStamped msg; - tf::transformStampedTFToMsg(viewtfs[i], msg); - long sec = msg.header.stamp.sec; - long nsec = msg.header.stamp.nsec; - double time = double(sec)+1e-9*double(nsec); - - Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]); - - cout << m << endl << endl; - - unrefined.push_back(m); - times.push_back(time); - - cv::Mat fullmask; - fullmask.create(480,640,CV_8UC1); - unsigned char * maskdata = (unsigned char *)fullmask.data; - for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} - masks.push_back(new reglib::ModelMask(fullmask)); - - reglib::Camera * cam = sweep->frames.front()->camera->clone(); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m);//a.matrix()); - frames.push_back(frame); - - //both_unrefined.push_back(sweep->frames.front()->pose.inverse()*a.matrix()); - both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); - } - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( sweep, reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; - sweep->points = mu->getSuperPoints(sweep->relativeposes,sweep->frames,sweep->modelmasks,1,false); - - //Not needed if metaroom well calibrated - reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); - bgmassreg->timeout = 20; - bgmassreg->viewer = viewer; - bgmassreg->use_surface = true; - bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = 0; - bgmassreg->maskstep = 5; - bgmassreg->nomaskstep = 5; - bgmassreg->nomask = true; - bgmassreg->stopval = 0.0005; - bgmassreg->addModel(sweep); - bgmassreg->setData(frames,masks); - reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); - - for(unsigned int i = 0; i < frames.size(); i++){ - frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; - } - - reglib::Model * fullmodel = new reglib::Model(); - fullmodel->frames = sweep->frames; - fullmodel->relativeposes = sweep->relativeposes; - fullmodel->modelmasks = sweep->modelmasks; - for(unsigned int i = 0; i < frames.size(); i++){ - fullmodel->frames.push_back(frames[i]); - fullmodel->modelmasks.push_back(masks[i]); - fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); - } - - delete bgmassreg; - delete reg; - delete mu; - delete sweep; - - return fullmodel; - */ -} - -int main(int argc, char** argv){ ros::init(argc, argv, "metaroom_additional_view_processing"); ros::NodeHandle n; + std::vector segsubs; + std::vector sendsubs; + int inputstate = 0; for(int i = 1; i < argc;i++){ printf("input: %s\n",argv[i]); @@ -1221,6 +1068,9 @@ int main(int argc, char** argv){ else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} + else if(std::string(argv[i]).compare("-sendModel") == 0){ inputstate = 8;} + else if(std::string(argv[i]).compare("-sendSub") == 0) { inputstate = 9;} + else if(std::string(argv[i]).compare("-sendTopic") == 0){ inputstate = 10;} else if(std::string(argv[i]).compare("-v") == 0){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0.5, 0, 0.5); @@ -1230,8 +1080,7 @@ int main(int argc, char** argv){ inputstate = 3; }else if(inputstate == 0){ out_pub = n.advertise(outtopic, 1000); - ros::Subscriber sub = n.subscribe(std::string(argv[i]), 1000, chatterCallback); - ros::spin(); + segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); }else if(inputstate == 1){ outtopic = std::string(argv[i]); }else if(inputstate == 3){ @@ -1245,12 +1094,21 @@ int main(int argc, char** argv){ }else if(inputstate == 7){ sweepPoses = readPoseXML(std::string(argv[i])); //exit(0); - }else{ + }else if(inputstate == 2){ out_pub = n.advertise(outtopic, 1000); - std::vector mods = loadModels(std::string(argv[i])); - exit(0); processMetaroom(std::string(argv[i])); + }else if(inputstate == 8){ + model_pub = n.advertise(modelouttopic, 1000); + sendMetaroomToServer(std::string(argv[i])); + }else if(inputstate == 9){ + model_pub = n.advertise(modelouttopic, 1000); + sendsubs.push_back(n.subscribe(std::string(argv[i]), 1000, sendCallback)); + }else if(inputstate == 10){ + modelouttopic = std::string(argv[i]); } } + + ros::spin(); + printf("done...\n"); return 0; } diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index cd4361ae..a7106b9e 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -740,10 +740,325 @@ show_sorted(); } } + +void addNewModel(reglib::Model * model){ + modeldatabase->add(model); + addToDB(modeldatabase, model,false); + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + publish_history(modeldatabase->models[i]->getHistory()); + } + + show_sorted(); + publishDatabasePCD(); + dumpDatabase(savepath); + + +// for(unsigned int m = 0; run_search && m < modeldatabase->models.size(); m++){ +// printf("looking at: %i\n",int(modeldatabase->models[m]->last_changed)); +// reglib::Model * currentTest = modeldatabase->models[m]; +// if(currentTest->last_changed > current_model_update_before){ +// printf("changed: %i\n",int(m)); + +// double start = getTime(); + +// new_search_result = false; +// models_new_pub.publish(getModelMSG(currentTest)); + +// while(getTime()-start < search_timeout){ +// ros::spinOnce(); +// if(new_search_result){ + +// for(unsigned int i = 0; i < sresult.retrieved_images.size(); i++){ + +// int maxval = 0; +// int maxind = 0; + +// int dilation_size = 0; +// cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, +// cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), +// cv::Point( dilation_size, dilation_size ) ); +// for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ +// //framekeys + +// cv_bridge::CvImagePtr ret_image_ptr; +// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} +// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +// cv_bridge::CvImagePtr ret_mask_ptr; +// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} +// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +// cv_bridge::CvImagePtr ret_depth_ptr; +// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} +// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +// cv::Mat rgbimage = ret_image_ptr->image; +// cv::Mat maskimage = ret_mask_ptr->image; +// cv::Mat depthimage = ret_depth_ptr->image; + +// cv::Mat erosion_dst; +// cv::erode( maskimage, erosion_dst, element ); + +// unsigned short * depthdata = (unsigned short * )depthimage.data; +// unsigned char * rgbdata = (unsigned char * )rgbimage.data; +// unsigned char * maskdata = (unsigned char * )erosion_dst.data; +// int count = 0; +// for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ +// if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ +// count++; +// } +// } +// if(count > maxval){ +// maxval = count; +// maxind = j; +// } +// } + +// //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ +// if(maxval > 100){//Atleast some pixels has to be in the mask... +// std::string key = sresult.retrieved_image_paths[i].strings[maxind]; +// printf("searchresult key:%s\n",key.c_str()); +// if(framekeys.count(key) == 0){ + +// int j = maxind; + +// cv_bridge::CvImagePtr ret_image_ptr; +// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} +// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +// cv_bridge::CvImagePtr ret_mask_ptr; +// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} +// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +// cv_bridge::CvImagePtr ret_depth_ptr; +// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} +// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +// cv::Mat rgbimage = ret_image_ptr->image; +// cv::Mat maskimage = ret_mask_ptr->image; +// cv::Mat depthimage = ret_depth_ptr->image; +// for (int ii = 0; ii < depthimage.rows; ++ii) { +// for (int jj = 0; jj < depthimage.cols; ++jj) { +// depthimage.at(ii, jj) *= 5; +// } +// } + + +// cv::Mat erosion_dst; +// cv::erode( maskimage, erosion_dst, element ); + +// // cv::Mat overlap = rgbimage.clone(); +// // unsigned short * depthdata = (unsigned short * )depthimage.data; +// // unsigned char * rgbdata = (unsigned char * )rgbimage.data; +// // unsigned char * maskdata = (unsigned char * )maskimage.data; +// // unsigned char * overlapdata = (unsigned char * )overlap.data; +// // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ +// // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ +// // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; +// // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; +// // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; +// // }else{ +// // overlapdata[3*pixel+0] = 0; +// // overlapdata[3*pixel+1] = 0; +// // overlapdata[3*pixel+2] = 0; +// // } +// // } +// // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); +// // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); +// // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); +// // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); + +// Eigen::Affine3d epose = Eigen::Affine3d::Identity(); +// reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); +// frame->keyval = key; + +// // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); +// // frame->show(true); +// //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); +// reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); +// bool res = searchmodel->testFrame(0); + +// reglib::Model * searchmodelHolder = new reglib::Model(); +// searchmodelHolder->submodels.push_back(searchmodel); +// searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); +// searchmodelHolder->last_changed = ++current_model_update; +// searchmodelHolder->recomputeModelPoints(); + +// //searchmodelHolder->showHistory(viewer); + + +// // if(cv::waitKey(0) != 'n'){ +// //Todo:: check new model is not flat or L shape + +// printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); +// //addToDB(modeldatabase, searchmodel,false,true); +// addToDB(modeldatabase, searchmodelHolder,false,true); +// show_sorted(); +// } +// } + + +// /* +//// for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ +//// //framekeys + +//// cv_bridge::CvImagePtr ret_image_ptr; +//// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} +//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +//// cv_bridge::CvImagePtr ret_mask_ptr; +//// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} +//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +//// cv_bridge::CvImagePtr ret_depth_ptr; +//// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} +//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +//// cv::Mat rgbimage = ret_image_ptr->image; +//// cv::Mat maskimage = ret_mask_ptr->image; +//// cv::Mat depthimage = ret_depth_ptr->image; + +//// cv::Mat erosion_dst; +//// cv::erode( maskimage, erosion_dst, element ); + +//// unsigned short * depthdata = (unsigned short * )depthimage.data; +//// unsigned char * rgbdata = (unsigned char * )rgbimage.data; +//// unsigned char * maskdata = (unsigned char * )erosion_dst.data; +//// int count = 0; +//// for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ +//// if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ +//// count++; +//// } +//// } +//// if(count > maxval){ +//// maxval = count; +//// maxind = j; +//// } +//// } + +//// //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ +//// if(maxval > 100){//Atleast some pixels has to be in the mask... +//// std::string key = sresult.retrieved_image_paths[i].strings[maxind]; +//// printf("searchresult key:%s\n",key.c_str()); +//// if(framekeys.count(key) == 0){ + +//// int j = maxind; + +//// cv_bridge::CvImagePtr ret_image_ptr; +//// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} +//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +//// cv_bridge::CvImagePtr ret_mask_ptr; +//// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} +//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +//// cv_bridge::CvImagePtr ret_depth_ptr; +//// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} +//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + +//// cv::Mat rgbimage = ret_image_ptr->image; +//// cv::Mat maskimage = ret_mask_ptr->image; +//// cv::Mat depthimage = ret_depth_ptr->image; +//// for (int ii = 0; ii < depthimage.rows; ++ii) { +//// for (int jj = 0; jj < depthimage.cols; ++jj) { +//// depthimage.at(ii, jj) *= 5; +//// } +//// } + + +//// cv::Mat erosion_dst; +//// cv::erode( maskimage, erosion_dst, element ); + +//// // cv::Mat overlap = rgbimage.clone(); +//// // unsigned short * depthdata = (unsigned short * )depthimage.data; +//// // unsigned char * rgbdata = (unsigned char * )rgbimage.data; +//// // unsigned char * maskdata = (unsigned char * )maskimage.data; +//// // unsigned char * overlapdata = (unsigned char * )overlap.data; +//// // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ +//// // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ +//// // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; +//// // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; +//// // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; +//// // }else{ +//// // overlapdata[3*pixel+0] = 0; +//// // overlapdata[3*pixel+1] = 0; +//// // overlapdata[3*pixel+2] = 0; +//// // } +//// // } +//// // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); +//// // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); +//// // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); +//// // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); + +//// Eigen::Affine3d epose = Eigen::Affine3d::Identity(); +//// reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); +//// frame->keyval = key; +//// frame->show(true); +//// //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); +//// reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); +//// bool res = searchmodel->testFrame(0); + +//// reglib::Model * searchmodelHolder = new reglib::Model(); +//// searchmodelHolder->submodels.push_back(searchmodel); +//// searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); +//// searchmodelHolder->last_changed = ++current_model_update; +//// searchmodelHolder->recomputeModelPoints(); + + +//// // if(cv::waitKey(0) != 'n'){ +//// //Todo:: check new model is not flat or L shape + +//// printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); +//// //addToDB(modeldatabase, searchmodel,false,true); +//// addToDB(modeldatabase, searchmodelHolder,false,true); +//// show_sorted(); +//// // } +//// } + +//// } +// */ +// } + +// break; +// }else{ +// printf("searching... timeout in %3.3f\n", +start +search_timeout - getTime()); +// usleep(100000); +// } +// } +// } +// //exit(0); +// } + +// for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ +// for(unsigned int j = 0; j < modeldatabase->models[i]->submodels.size(); j++){ +// if( modeldatabase->models[i]->submodels[j]->frames.front() == frontframe){ +// Eigen::Matrix4d pose = frontframe->pose * modeldatabase->models[i]->submodels_relativeposes[j].inverse(); + +// pcl::PointCloud::Ptr cloud = modeldatabase->models[i]->submodels[j]->getPCLcloud(1,true); +// pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); +// pcl::transformPointCloud (*cloud, *transformed_cloud, pose); + + +// sensor_msgs::PointCloud2 input; +// pcl::toROSMsg (*transformed_cloud,input);//, *transformed_cloud); +// input.header.frame_id = "/map"; +// model_last_pub.publish(input); +// } +// } +// } + +// newmodel = 0; +// sweepid_counter++; + +} + +void modelCallback(const quasimodo_msgs::model & m){ + addNewModel(getModelFromMSG(m)); +} + bool nextNew = true; reglib::Model * newmodel = 0; - //std::vector newmasks; std::vector allmasks; @@ -1235,7 +1550,6 @@ int getdir (std::string dir, std::vector & files){ void clearMem(){ - for(auto iterator = cameras.begin(); iterator != cameras.end(); iterator++) { delete iterator->second; } @@ -1299,6 +1613,8 @@ int main(int argc, char **argv){ model_last_pub = n.advertise("modelserver/last", 1000); model_places_pub = n.advertise("modelserver/model_places", 1000); + std::vector input_model_subs; + int inputstate = -1; for(int i = 1; i < argc;i++){ printf("input: %s\n",argv[i]); @@ -1321,7 +1637,8 @@ int main(int argc, char **argv){ else if(std::string(argv[i]).compare("-v_register") == 0 || std::string(argv[i]).compare("-v_reg") == 0){ printf("visualization registration turned on\n"); visualization = true; inputstate = 10;} else if(std::string(argv[i]).compare("-v_scoring") == 0 || std::string(argv[i]).compare("-v_score") == 0 || std::string(argv[i]).compare("-v_sco") == 0){ printf("visualization scoring turned on\n"); visualization = true; show_scoring = true;} else if(std::string(argv[i]).compare("-v_db") == 0){ printf("visualization db turned on\n"); visualization = true; show_db = true;} - else if(inputstate == 1){ + else if(std::string(argv[i]).compare("-intopic") == 0){ printf("intopic input state\n"); inputstate = 11;} + else if(inputstate == 1){ reglib::Camera * cam = reglib::Camera::load(std::string(argv[i])); delete cameras[0]; cameras[0] = cam; @@ -1347,9 +1664,12 @@ int main(int argc, char **argv){ show_init_lvl = atoi(argv[i]); }else if(inputstate == 9){ show_refine_lvl = atoi(argv[i]); - }else if(inputstate == 10){ - show_reg_lvl = atoi(argv[i]); - } + }else if(inputstate == 10){ + show_reg_lvl = atoi(argv[i]); + }else if(inputstate == 11){ + printf("adding %s to input_model_subs\n",argv[i]); + input_model_subs.push_back(n.subscribe(std::string(argv[i]), 100, modelCallback)); + } } if(visualization){ diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 3d662876..7198154e 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -56,6 +56,7 @@ namespace reglib void savePCD(std::string path = "cloud.pcd", Eigen::Matrix4d pose = Eigen::Matrix4d::Identity()); void save(std::string path = ""); + RGBDFrame * clone(); static RGBDFrame * load(Camera * cam, std::string path); }; diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 659fb9f3..a584ff7f 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -289,6 +289,10 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int return probs; } +RGBDFrame * RGBDFrame::clone(){ + return new RGBDFrame(camera->clone(), rgb.clone(),depth.clone(),capturetime, pose, true); +} + RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals){ keyval = ""; diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 4fa77ebd..9aa08789 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -39,2045 +39,2045 @@ namespace reglib { double mysign(double v){ - if(v < 0){return -1;} - return 1; + if(v < 0){return -1;} + return 1; } double getTime(){ - struct timeval start1; - gettimeofday(&start1, NULL); - return double(start1.tv_sec+(start1.tv_usec/1000000.0)); + struct timeval start1; + gettimeofday(&start1, NULL); + return double(start1.tv_sec+(start1.tv_usec/1000000.0)); } float graph_cut(vector& graphs_out,vector>& second_graphinds, Graph& graph_in, std::vector graph_inds){ - using adjacency_iterator = boost::graph_traits::adjacency_iterator; - typename boost::property_map::type vertex_id = boost::get(boost::vertex_index, graph_in); - typename boost::property_map::type edge_id = boost::get(boost::edge_weight, graph_in); - typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, graph_in); - - BOOST_AUTO(parities, boost::make_one_bit_color_map(boost::num_vertices(graph_in), boost::get(boost::vertex_index, graph_in))); - - float w = boost::stoer_wagner_min_cut(graph_in, boost::get(boost::edge_weight, graph_in), boost::parity_map(parities)); - - unordered_map mappings; - VertexIndex counters[2] = {0, 0}; - - graphs_out.push_back(new Graph(1)); - graphs_out.push_back(new Graph(1)); - second_graphinds.push_back(vector()); - second_graphinds.push_back(vector()); - //std::cout << "One set of vertices consists of:" << std::endl; - bool flag; - Edge edge; - for (size_t i = 0; i < boost::num_vertices(graph_in); ++i) { - int first = boost::get(parities, i); - second_graphinds[first].push_back(graph_inds[i]); - // iterate adjacent edges - adjacency_iterator ai, ai_end; - for (tie(ai, ai_end) = boost::adjacent_vertices(i, graph_in); ai != ai_end; ++ai) { - VertexIndex neighbor_index = boost::get(vertex_id, *ai); - int second = boost::get(parities, neighbor_index); - if (first == second && neighbor_index < i) { - tie(edge, flag) = boost::edge(i, neighbor_index, graph_in); - edge_weight_property weight = boost::get(edge_id, edge); - if (mappings.count(i) == 0) { - mappings[i] = counters[first]++; - } - if (mappings.count(neighbor_index) == 0) { - mappings[neighbor_index] = counters[first]++; - } - tie(edge, flag) = boost::add_edge(mappings[neighbor_index], mappings[i], weight, *graphs_out[first]); - - typename boost::property_map::type vertex_name_first = boost::get(boost::vertex_name, *graphs_out[first]); - boost::get(vertex_name_first, mappings[i]) = boost::get(vertex_name, i); - boost::get(vertex_name_first, mappings[neighbor_index]) = boost::get(vertex_name, *ai); - } - } - } - return w; + using adjacency_iterator = boost::graph_traits::adjacency_iterator; + typename boost::property_map::type vertex_id = boost::get(boost::vertex_index, graph_in); + typename boost::property_map::type edge_id = boost::get(boost::edge_weight, graph_in); + typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, graph_in); + + BOOST_AUTO(parities, boost::make_one_bit_color_map(boost::num_vertices(graph_in), boost::get(boost::vertex_index, graph_in))); + + float w = boost::stoer_wagner_min_cut(graph_in, boost::get(boost::edge_weight, graph_in), boost::parity_map(parities)); + + unordered_map mappings; + VertexIndex counters[2] = {0, 0}; + + graphs_out.push_back(new Graph(1)); + graphs_out.push_back(new Graph(1)); + second_graphinds.push_back(vector()); + second_graphinds.push_back(vector()); + //std::cout << "One set of vertices consists of:" << std::endl; + bool flag; + Edge edge; + for (size_t i = 0; i < boost::num_vertices(graph_in); ++i) { + int first = boost::get(parities, i); + second_graphinds[first].push_back(graph_inds[i]); + // iterate adjacent edges + adjacency_iterator ai, ai_end; + for (tie(ai, ai_end) = boost::adjacent_vertices(i, graph_in); ai != ai_end; ++ai) { + VertexIndex neighbor_index = boost::get(vertex_id, *ai); + int second = boost::get(parities, neighbor_index); + if (first == second && neighbor_index < i) { + tie(edge, flag) = boost::edge(i, neighbor_index, graph_in); + edge_weight_property weight = boost::get(edge_id, edge); + if (mappings.count(i) == 0) { + mappings[i] = counters[first]++; + } + if (mappings.count(neighbor_index) == 0) { + mappings[neighbor_index] = counters[first]++; + } + tie(edge, flag) = boost::add_edge(mappings[neighbor_index], mappings[i], weight, *graphs_out[first]); + + typename boost::property_map::type vertex_name_first = boost::get(boost::vertex_name, *graphs_out[first]); + boost::get(vertex_name_first, mappings[i]) = boost::get(vertex_name, i); + boost::get(vertex_name_first, mappings[neighbor_index]) = boost::get(vertex_name, *ai); + } + } + } + return w; } float recursive_split(std::vector * graphs_out,std::vector> * graphinds_out, Graph * graph, std::vector graph_inds){ - if(boost::num_vertices(*graph) == 1){ - graphs_out->push_back(graph); - graphinds_out->push_back(graph_inds); - return 0; - } - - vector second_graphs; - vector> second_graphinds; - float w = graph_cut(second_graphs,second_graphinds,*graph,graph_inds); - if(w <= 0){ - delete graph; - return 2*w + recursive_split(graphs_out, graphinds_out,second_graphs.front(),second_graphinds.front()) + recursive_split(graphs_out, graphinds_out, second_graphs.back(),second_graphinds.back()); - }else{ - graphs_out->push_back(graph); - graphinds_out->push_back(graph_inds); - delete second_graphs.front(); - delete second_graphs.back(); - return 0; - } + if(boost::num_vertices(*graph) == 1){ + graphs_out->push_back(graph); + graphinds_out->push_back(graph_inds); + return 0; + } + + vector second_graphs; + vector> second_graphinds; + float w = graph_cut(second_graphs,second_graphinds,*graph,graph_inds); + if(w <= 0){ + delete graph; + return 2*w + recursive_split(graphs_out, graphinds_out,second_graphs.front(),second_graphinds.front()) + recursive_split(graphs_out, graphinds_out, second_graphs.back(),second_graphinds.back()); + }else{ + graphs_out->push_back(graph); + graphinds_out->push_back(graph_inds); + delete second_graphs.front(); + delete second_graphs.back(); + return 0; + } } std::vector partition_graph(std::vector< std::vector< float > > & scores){ - int nr_data = scores.size(); - Graph* graph = new Graph(nr_data); - std::vector graph_inds; - graph_inds.resize(nr_data); - - typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, *graph); - - float sum = 0; - for(int i = 0; i < nr_data; i++){ - graph_inds[i] = i; - for(int j = i+1; j < nr_data; j++){ - float weight = scores[i][j]; - if(weight != 0){ - sum += 2*weight; - edge_weight_property e = weight; - boost::add_edge(i, j, e, *graph); - } - } - } - - std::vector * graphs_out = new std::vector(); - std::vector> * graphinds_out = new std::vector>(); - float best = sum-recursive_split(graphs_out,graphinds_out, graph,graph_inds ); - - std::vector part; - part.resize(nr_data); - //int * part = new int[nr_data]; - for(unsigned int i = 0; i < graphinds_out->size(); i++){ - for(unsigned int j = 0; j < graphinds_out->at(i).size(); j++){ - part[graphinds_out->at(i).at(j)] = i; - } - } - return part; + int nr_data = scores.size(); + Graph* graph = new Graph(nr_data); + std::vector graph_inds; + graph_inds.resize(nr_data); + + typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, *graph); + + float sum = 0; + for(int i = 0; i < nr_data; i++){ + graph_inds[i] = i; + for(int j = i+1; j < nr_data; j++){ + float weight = scores[i][j]; + if(weight != 0){ + sum += 2*weight; + edge_weight_property e = weight; + boost::add_edge(i, j, e, *graph); + } + } + } + + std::vector * graphs_out = new std::vector(); + std::vector> * graphinds_out = new std::vector>(); + float best = sum-recursive_split(graphs_out,graphinds_out, graph,graph_inds ); + + std::vector part; + part.resize(nr_data); + //int * part = new int[nr_data]; + for(unsigned int i = 0; i < graphinds_out->size(); i++){ + for(unsigned int j = 0; j < graphinds_out->at(i).size(); j++){ + part[graphinds_out->at(i).at(j)] = i; + } + } + return part; } std::vector ModelUpdater::getPartition(std::vector< std::vector< float > > & scores, int dims, int nr_todo, double timelimit){ - return partition_graph(scores); + return partition_graph(scores); } ModelUpdater::ModelUpdater(){ - occlusion_penalty = 10; - massreg_timeout = 60; - model = 0; - show_init_lvl = 0;//init show - show_refine_lvl = 0;//refine show - show_scoring = false;//fuse scoring show + occlusion_penalty = 10; + massreg_timeout = 60; + model = 0; + show_init_lvl = 0;//init show + show_refine_lvl = 0;//refine show + show_scoring = false;//fuse scoring show } ModelUpdater::ModelUpdater(Model * model_){ - occlusion_penalty = 10; - massreg_timeout = 60; - show_init_lvl = 0;//init show - show_refine_lvl = 0;//refine show - show_scoring = false;//fuse scoring show - model = model_; + occlusion_penalty = 10; + massreg_timeout = 60; + show_init_lvl = 0;//init show + show_refine_lvl = 0;//refine show + show_scoring = false;//fuse scoring show + model = model_; } ModelUpdater::~ModelUpdater(){} FusionResults ModelUpdater::registerModel(Model * model2, Eigen::Matrix4d guess, double uncertanity){return FusionResults();} void ModelUpdater::fuse(Model * model2, Eigen::Matrix4d guess, double uncertanity){ - for(unsigned int i = 0; i < model2->frames.size();i++){ - model->frames.push_back(model2->frames[i]); - //model->masks.push_back(model2->masks[i]); - model->modelmasks.push_back(model2->modelmasks[i]); - model->relativeposes.push_back(guess*model2->relativeposes[i]); - } + for(unsigned int i = 0; i < model2->frames.size();i++){ + model->frames.push_back(model2->frames[i]); + //model->masks.push_back(model2->masks[i]); + model->modelmasks.push_back(model2->modelmasks[i]); + model->relativeposes.push_back(guess*model2->relativeposes[i]); + } } UpdatedModels ModelUpdater::fuseData(FusionResults * f, Model * model1,Model * model2){return UpdatedModels();} bool ModelUpdater::isRefinementNeeded(){ - vector > occlusionScores; - occlusionScores.resize(model->frames.size()); - for(unsigned int i = 0; i < model->frames.size(); i++){occlusionScores[i].resize(model->frames.size());} - - - for(unsigned int i = 0; i < model->frames.size(); i++){ - for(unsigned int j = i+1; j < model->frames.size(); j++){ - Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; - occlusionScores[j][i] = computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,false); - occlusionScores[i][j] = computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,false); - } - } - - - std::vector > scores = getScores(occlusionScores); - std::vector partition = getPartition(scores,2,5,2); - - bool failed = false; - for(unsigned int i = 0; i < partition.size(); i++){failed = failed | (partition[i]!=0);} - - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - if(scores[i][j] > 0){printf(" ");} - printf("%5.5f ",0.0001*scores[i][j]); - // if(scores[i][j] < 0){ - // Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; - // computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,true); - // computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,true); - // } - } - printf("\n"); - } - - printf("partition"); - for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} - printf("\n"); - return failed; + vector > occlusionScores; + occlusionScores.resize(model->frames.size()); + for(unsigned int i = 0; i < model->frames.size(); i++){occlusionScores[i].resize(model->frames.size());} + + + for(unsigned int i = 0; i < model->frames.size(); i++){ + for(unsigned int j = i+1; j < model->frames.size(); j++){ + Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; + occlusionScores[j][i] = computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,false); + occlusionScores[i][j] = computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,false); + } + } + + + std::vector > scores = getScores(occlusionScores); + std::vector partition = getPartition(scores,2,5,2); + + bool failed = false; + for(unsigned int i = 0; i < partition.size(); i++){failed = failed | (partition[i]!=0);} + + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + if(scores[i][j] > 0){printf(" ");} + printf("%5.5f ",0.0001*scores[i][j]); + // if(scores[i][j] < 0){ + // Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; + // computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,true); + // computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,true); + // } + } + printf("\n"); + } + + printf("partition"); + for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} + printf("\n"); + return failed; } OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step, bool debugg){ - // printf("start:: %s::%i\n",__FILE__,__LINE__); - OcclusionScore oc; - - unsigned char * dst_maskdata = (unsigned char *)(cm->mask.data); - unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); - float * dst_normalsdata = (float *)(cf->normals.data); - - float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); - float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); - float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); - - Camera * dst_camera = cf->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - - int nr_data = spvec.size(); - - std::vector residuals; - std::vector debugg_src_inds; - std::vector debugg_dst_inds; - std::vector weights; - residuals.reserve(nr_data); - if(debugg){ - debugg_src_inds.reserve(nr_data); - debugg_dst_inds.reserve(nr_data); - } - weights.reserve(nr_data); - - for(unsigned int ind = 0; ind < spvec.size();ind+=step){ - superpoint & sp = spvec[ind]; - - float src_x = sp.point(0); - float src_y = sp.point(1); - float src_z = sp.point(2); - - float src_nx = sp.normal(0); - float src_ny = sp.normal(1); - float src_nz = sp.normal(2); - - float point_information = sp.point_information; - - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - - double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - double dst_noise = dst_z * dst_z; - double point_noise = 1.0/sqrt(point_information); - - double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); - d *= compare_mul; - - double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); - double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); - - residuals.push_back(d); - weights.push_back(angle_dst*angle_dst*angle_dst); - if(debugg){ - debugg_src_inds.push_back(ind); - debugg_dst_inds.push_back(dst_ind); - } - } - } - } - - // DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); - // func->maxp = 1.0; - // func->update_size = true; - // func->zeromean = true; - // func->startreg = 0.0001; - // func->debugg_print = debugg; - // func->bidir = true; - // func->maxnoise = pred; - // func->reset(); - - DistanceWeightFunction2 * func = new DistanceWeightFunction2(); - func->f = THRESHOLD; - func->p = 0.01; - - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); - for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} - func->computeModel(X); - - Eigen::VectorXd W = func->getProbs(X); - - delete func; - - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - oc.score += weight*weights.at(i); - oc.occlusions += ocl*weights.at(i); - } - - - if(debugg){ - //cf->show(true); - - pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); - dcloud->points.resize(dst_width*dst_height); - for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - unsigned int dst_ind = dst_h*dst_width+dst_w; - float z = dst_idepth*float(dst_depthdata[dst_ind]); - if(z > 0){ - float x = (float(dst_w) - dst_cx) * z * dst_ifx; - float y = (float(dst_h) - dst_cy) * z * dst_ify; - dcloud->points[dst_ind].x = x; - dcloud->points[dst_ind].y = y; - dcloud->points[dst_ind].z = z; - dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; - dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; - dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; - if(dst_w % 10 == 0 && dst_h % 10 == 0){ - //printf("%i %i -> %4.4f %4.4f %4.4f\n",dst_w,dst_h,dst_normalsdata[3*dst_ind+0],dst_h,dst_normalsdata[3*dst_ind+1],dst_h,dst_normalsdata[3*dst_ind+2]); - } - //if(false && dst_maskdata[dst_ind] == 255){ - if(dst_maskdata[dst_ind] == 255){ - dcloud->points[dst_ind].r = 255; - dcloud->points[dst_ind].g = 000; - dcloud->points[dst_ind].b = 255; - } - } - } - } - - oc.print(); - pcl::PointCloud::Ptr scloud (new pcl::PointCloud); - for(unsigned int i = 0; i < spvec.size(); i++){ - superpoint & sp = spvec[i]; - pcl::PointXYZRGBNormal p; - p.x = sp.point(0); - p.y = sp.point(1); - p.z = sp.point(2); - - float tx = m00*p.x + m01*p.y + m02*p.z + m03; - float ty = m10*p.x + m11*p.y + m12*p.z + m13; - float tz = m20*p.x + m21*p.y + m22*p.z + m23; - - p.x = tx; - p.y = ty; - p.z = tz; - - p.normal_x = sp.normal(0); - p.normal_y = sp.normal(1); - p.normal_z = sp.normal(2); - p.r = sp.feature(0); - p.g = sp.feature(1); - p.b = sp.feature(2); - - scloud->points.push_back(p); - } - - // viewer->removeAllPointClouds(); - // viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - // viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - // viewer->spin(); - - viewer->removeAllPointClouds(); - viewer->removeAllShapes(); - - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - if(debugg){ - unsigned int src_ind = debugg_src_inds[i]; - unsigned int dst_ind = debugg_dst_inds[i]; - if(ocl > 0.01 || weight > 0.01){ - scloud->points[src_ind].r = 255.0*ocl*weights.at(i); - scloud->points[src_ind].g = 255.0*weight*weights.at(i); - scloud->points[src_ind].b = 0; - - if(i % 300 == 0){ - char buf [1024]; - sprintf(buf,"line%i",i); - viewer->addLine (scloud->points[src_ind], dcloud->points[dst_ind],buf); - } - }else{ - scloud->points[src_ind].x = 0; - scloud->points[src_ind].y = 0; - scloud->points[src_ind].z = 0; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud"); - - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - viewer->spin(); - viewer->removeAllPointClouds(); - viewer->removeAllShapes(); - - } - //printf("stop :: %s::%i\n",__FILE__,__LINE__); - return oc; + // printf("start:: %s::%i\n",__FILE__,__LINE__); + OcclusionScore oc; + + unsigned char * dst_maskdata = (unsigned char *)(cm->mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); + float * dst_normalsdata = (float *)(cf->normals.data); + + float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); + float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); + float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); + + Camera * dst_camera = cf->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + int nr_data = spvec.size(); + + std::vector residuals; + std::vector debugg_src_inds; + std::vector debugg_dst_inds; + std::vector weights; + residuals.reserve(nr_data); + if(debugg){ + debugg_src_inds.reserve(nr_data); + debugg_dst_inds.reserve(nr_data); + } + weights.reserve(nr_data); + + for(unsigned int ind = 0; ind < spvec.size();ind+=step){ + superpoint & sp = spvec[ind]; + + float src_x = sp.point(0); + float src_y = sp.point(1); + float src_z = sp.point(2); + + float src_nx = sp.normal(0); + float src_ny = sp.normal(1); + float src_nz = sp.normal(2); + + float point_information = sp.point_information; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + double dst_noise = dst_z * dst_z; + double point_noise = 1.0/sqrt(point_information); + + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; + + double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); + double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); + + residuals.push_back(d); + weights.push_back(angle_dst*angle_dst*angle_dst); + if(debugg){ + debugg_src_inds.push_back(ind); + debugg_dst_inds.push_back(dst_ind); + } + } + } + } + + // DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + // func->maxp = 1.0; + // func->update_size = true; + // func->zeromean = true; + // func->startreg = 0.0001; + // func->debugg_print = debugg; + // func->bidir = true; + // func->maxnoise = pred; + // func->reset(); + + DistanceWeightFunction2 * func = new DistanceWeightFunction2(); + func->f = THRESHOLD; + func->p = 0.01; + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); + for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} + func->computeModel(X); + + Eigen::VectorXd W = func->getProbs(X); + + delete func; + + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + oc.score += weight*weights.at(i); + oc.occlusions += ocl*weights.at(i); + } + + + if(debugg){ + //cf->show(true); + + pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + dcloud->points.resize(dst_width*dst_height); + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + unsigned int dst_ind = dst_h*dst_width+dst_w; + float z = dst_idepth*float(dst_depthdata[dst_ind]); + if(z > 0){ + float x = (float(dst_w) - dst_cx) * z * dst_ifx; + float y = (float(dst_h) - dst_cy) * z * dst_ify; + dcloud->points[dst_ind].x = x; + dcloud->points[dst_ind].y = y; + dcloud->points[dst_ind].z = z; + dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; + dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; + dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; + if(dst_w % 10 == 0 && dst_h % 10 == 0){ + //printf("%i %i -> %4.4f %4.4f %4.4f\n",dst_w,dst_h,dst_normalsdata[3*dst_ind+0],dst_h,dst_normalsdata[3*dst_ind+1],dst_h,dst_normalsdata[3*dst_ind+2]); + } + //if(false && dst_maskdata[dst_ind] == 255){ + if(dst_maskdata[dst_ind] == 255){ + dcloud->points[dst_ind].r = 255; + dcloud->points[dst_ind].g = 000; + dcloud->points[dst_ind].b = 255; + } + } + } + } + + oc.print(); + pcl::PointCloud::Ptr scloud (new pcl::PointCloud); + for(unsigned int i = 0; i < spvec.size(); i++){ + superpoint & sp = spvec[i]; + pcl::PointXYZRGBNormal p; + p.x = sp.point(0); + p.y = sp.point(1); + p.z = sp.point(2); + + float tx = m00*p.x + m01*p.y + m02*p.z + m03; + float ty = m10*p.x + m11*p.y + m12*p.z + m13; + float tz = m20*p.x + m21*p.y + m22*p.z + m23; + + p.x = tx; + p.y = ty; + p.z = tz; + + p.normal_x = sp.normal(0); + p.normal_y = sp.normal(1); + p.normal_z = sp.normal(2); + p.r = sp.feature(0); + p.g = sp.feature(1); + p.b = sp.feature(2); + + scloud->points.push_back(p); + } + + // viewer->removeAllPointClouds(); + // viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + // viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + // viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->removeAllShapes(); + + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + unsigned int src_ind = debugg_src_inds[i]; + unsigned int dst_ind = debugg_dst_inds[i]; + if(ocl > 0.01 || weight > 0.01){ + scloud->points[src_ind].r = 255.0*ocl*weights.at(i); + scloud->points[src_ind].g = 255.0*weight*weights.at(i); + scloud->points[src_ind].b = 0; + + if(i % 300 == 0){ + char buf [1024]; + sprintf(buf,"line%i",i); + viewer->addLine (scloud->points[src_ind], dcloud->points[dst_ind],buf); + } + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud"); + + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + viewer->removeAllShapes(); + + } + //printf("stop :: %s::%i\n",__FILE__,__LINE__); + return oc; } OcclusionScore ModelUpdater::computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp, int step, bool debugg){ - OcclusionScore ocs; - for(unsigned int i = 0; i < cp.size(); i++){ - ocs.add(computeOcclusionScore(mod->points,cp[i].inverse()*rp,cf[i],cm[i],step,debugg)); - } - return ocs; + OcclusionScore ocs; + for(unsigned int i = 0; i < cp.size(); i++){ + ocs.add(computeOcclusionScore(mod->points,cp[i].inverse()*rp,cf[i],cm[i],step,debugg)); + } + return ocs; } OcclusionScore ModelUpdater::computeOcclusionScore(Model * model1, Model * model2, Matrix4d rp, int step, bool debugg){ - OcclusionScore ocs; - // ocs.add(computeOcclusionScore(model1, model2->rep_relativeposes,model2->rep_frames,model2->rep_modelmasks,rp.inverse(),step,debugg)); - // ocs.add(computeOcclusionScore(model2, model1->rep_relativeposes,model1->rep_frames,model1->rep_modelmasks,rp,step,debugg)); - ocs.add(computeOcclusionScore(model1, model2->relativeposes,model2->frames,model2->modelmasks,rp.inverse(),step,debugg)); - ocs.add(computeOcclusionScore(model2, model1->relativeposes,model1->frames,model1->modelmasks,rp,step,debugg)); - return ocs; + OcclusionScore ocs; + // ocs.add(computeOcclusionScore(model1, model2->rep_relativeposes,model2->rep_frames,model2->rep_modelmasks,rp.inverse(),step,debugg)); + // ocs.add(computeOcclusionScore(model2, model1->rep_relativeposes,model1->rep_frames,model1->rep_modelmasks,rp,step,debugg)); + ocs.add(computeOcclusionScore(model1, model2->relativeposes,model2->frames,model2->modelmasks,rp.inverse(),step,debugg)); + ocs.add(computeOcclusionScore(model2, model1->relativeposes,model1->frames,model1->modelmasks,rp,step,debugg)); + return ocs; } vector > ModelUpdater::computeOcclusionScore(vector models, vector rps, int step, bool debugg){ - unsigned int nr_models = models.size(); - vector > occlusionScores; - occlusionScores.resize(nr_models); - for(unsigned int i = 0; i < nr_models; i++){occlusionScores[i].resize(nr_models);} - - for(unsigned int i = 0; i < nr_models; i++){ - for(unsigned int j = i+1; j < nr_models; j++){ - //printf("%5.5f %5.5f\n",double(i+1)/double(nr_models),double(j+1)/double(nr_models)); - Eigen::Matrix4d relative_pose = rps[i].inverse() * rps[j]; - occlusionScores[i][j] = computeOcclusionScore(models[i],models[j], relative_pose, step, debugg); - occlusionScores[j][i] = occlusionScores[i][j]; - } - } - - return occlusionScores; + unsigned int nr_models = models.size(); + vector > occlusionScores; + occlusionScores.resize(nr_models); + for(unsigned int i = 0; i < nr_models; i++){occlusionScores[i].resize(nr_models);} + + for(unsigned int i = 0; i < nr_models; i++){ + for(unsigned int j = i+1; j < nr_models; j++){ + //printf("%5.5f %5.5f\n",double(i+1)/double(nr_models),double(j+1)/double(nr_models)); + Eigen::Matrix4d relative_pose = rps[i].inverse() * rps[j]; + occlusionScores[i][j] = computeOcclusionScore(models[i],models[j], relative_pose, step, debugg); + occlusionScores[j][i] = occlusionScores[i][j]; + } + } + + return occlusionScores; } double ModelUpdater::computeOcclusionScoreCosts(vector models){ - double total = 0; + double total = 0; - unsigned int nr_models = models.size(); - for(unsigned int i = 0; i < nr_models; i++){ - for(unsigned int j = 0; j < nr_models; j++){ - if(i == j){continue;} - total += models[j]->rep_frames.size() * models[i]->points.size(); - } - } + unsigned int nr_models = models.size(); + for(unsigned int i = 0; i < nr_models; i++){ + for(unsigned int j = 0; j < nr_models; j++){ + if(i == j){continue;} + total += models[j]->rep_frames.size() * models[i]->points.size(); + } + } - return total; + return total; } void ModelUpdater::addModelsToVector(vector & models, vector & rps, Model * model, Matrix4d rp){ - if(model->frames.size() > 0){ - models.push_back(model); - rps.push_back(rp); - } - - for(unsigned int i = 0; i < model->submodels.size(); i++){ - addModelsToVector(models,rps,model->submodels[i], rp*model->submodels_relativeposes[i]); - } + if(model->frames.size() > 0){ + models.push_back(model); + rps.push_back(rp); + } + + for(unsigned int i = 0; i < model->submodels.size(); i++){ + addModelsToVector(models,rps,model->submodels[i], rp*model->submodels_relativeposes[i]); + } } pcl::PointCloud::Ptr ModelUpdater::getPCLnormalcloud(vector & points){ - pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); - for(unsigned int i = 0; i < points.size(); i++){ - superpoint & sp = points[i]; - pcl::PointXYZRGBNormal p; - p.x = sp.point(0); - p.y = sp.point(1); - p.z = sp.point(2); - - p.normal_x = sp.normal(0); - p.normal_y = sp.normal(1); - p.normal_z = sp.normal(2); - p.r = sp.feature(0); - p.g = sp.feature(1); - p.b = sp.feature(2); - - cloud_ptr->points.push_back(p); - } - return cloud_ptr; + pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); + for(unsigned int i = 0; i < points.size(); i++){ + superpoint & sp = points[i]; + pcl::PointXYZRGBNormal p; + p.x = sp.point(0); + p.y = sp.point(1); + p.z = sp.point(2); + + p.normal_x = sp.normal(0); + p.normal_y = sp.normal(1); + p.normal_z = sp.normal(2); + p.r = sp.feature(0); + p.g = sp.feature(1); + p.b = sp.feature(2); + + cloud_ptr->points.push_back(p); + } + return cloud_ptr; } bool ModelUpdater::refineIfNeeded(){ - if(isRefinementNeeded()){ - MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1); - massreg->timeout = massreg_timeout; - massreg->viewer = viewer; - massreg->visualizationLvl = 1; - massreg->maskstep = 4; - massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); - - massreg->nomask = true; - massreg->stopval = 0.001; - - massreg->setData(model->frames,model->modelmasks); - MassFusionResults mfr = massreg->getTransforms(model->relativeposes); - model->relativeposes = mfr.poses; - - isRefinementNeeded(); - } - - - MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1); - massreg->timeout = massreg_timeout; - massreg->viewer = viewer; - massreg->visualizationLvl = 1; - massreg->maskstep = 4; - massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); - - massreg->nomask = true; - massreg->stopval = 0.001; - - massreg->setData(model->frames,model->modelmasks); - MassFusionResults mfr = massreg->getTransforms(model->relativeposes); - model->relativeposes = mfr.poses; - - vector spvec = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); - vector cp; - vector cf; - vector cm; - for(unsigned int i = 0; i < model->relativeposes.size(); i++){ - cp.push_back(model->relativeposes[i]); - cf.push_back(model->frames[i]); - cm.push_back(model->modelmasks[i]); - } - getGoodCompareFrames(cp,cf,cm); - - vector spvec2 = getSuperPoints(cp,cf,cm,1,true); - return true; -} + if(isRefinementNeeded()){ + MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1); + massreg->timeout = massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = 1; + massreg->maskstep = 4; + massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); -void ModelUpdater::makeInitialSetup(){ + massreg->nomask = true; + massreg->stopval = 0.001; - if(model->relativeposes.size() <= 1){ - model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); + massreg->setData(model->frames,model->modelmasks); + MassFusionResults mfr = massreg->getTransforms(model->relativeposes); + model->relativeposes = mfr.poses; - vector cp; - vector cf; - vector cm; - for(unsigned int i = 0; i < model->relativeposes.size(); i++){ - cp.push_back(model->relativeposes[i]); - cf.push_back(model->frames[i]); - cm.push_back(model->modelmasks[i]); - } - getGoodCompareFrames(cp,cf,cm); + isRefinementNeeded(); + } - model->rep_relativeposes = cp; - model->rep_frames = cf; - model->rep_modelmasks = cm; - return ; - } - int minHessian = 400; + MassRegistrationPPR * massreg = new MassRegistrationPPR(0.1); + massreg->timeout = massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = 1; + massreg->maskstep = 4; + massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); + + massreg->nomask = true; + massreg->stopval = 0.001; + + massreg->setData(model->frames,model->modelmasks); + MassFusionResults mfr = massreg->getTransforms(model->relativeposes); + model->relativeposes = mfr.poses; + + vector spvec = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); + vector cp; + vector cf; + vector cm; + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + cp.push_back(model->relativeposes[i]); + cf.push_back(model->frames[i]); + cm.push_back(model->modelmasks[i]); + } + getGoodCompareFrames(cp,cf,cm); - // cv::SurfFeatureDetector detector( minHessian ); - // cv::SurfDescriptorExtractor extractor; + vector spvec2 = getSuperPoints(cp,cf,cm,1,true); + return true; +} +void ModelUpdater::makeInitialSetup(){ - //cv::ORB orb = cv::ORB(250);//,1.2f, 1, 3, 0,2, cv::ORB::HARRIS_SCORE, 31); - cv::ORB orb = cv::ORB(10);//,1.2f, 1, 3, 0,2, cv::ORB::HARRIS_SCORE, 31); - // for(unsigned int i = 0; i < model->frames.size(); i++){ - // std::vector keypoints; - // cv::Mat descriptors; - // //orb(model->frames[i]->rgb, model->modelmasks[i]->getMask(), keypoints, descriptors); - // orb(model->frames[i]->rgb, cv::Mat(), keypoints, descriptors); - // model->all_keypoints.push_back(keypoints); - // model->all_descriptors.push_back(descriptors); + if(model->relativeposes.size() <= 1){ + model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); - // printf("keypoints: %i\n",keypoints.size()); + vector cp; + vector cf; + vector cm; + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + cp.push_back(model->relativeposes[i]); + cf.push_back(model->frames[i]); + cm.push_back(model->modelmasks[i]); + } + getGoodCompareFrames(cp,cf,cm); + model->rep_relativeposes = cp; + model->rep_frames = cf; + model->rep_modelmasks = cm; + return ; + } - // cv::Mat descriptors_surf; - // std::vector keypoints_surf; + int minHessian = 400; - // detector.detect(model->frames[i]->rgb, keypoints_surf, model->modelmasks[i]->getMask() ); - // extractor.compute( model->frames[i]->rgb, keypoints_surf, descriptors_surf ); - // model->all_keypoints.push_back(keypoints_surf); - // model->all_descriptors.push_back(descriptors_surf); + // cv::SurfFeatureDetector detector( minHessian ); + // cv::SurfDescriptorExtractor extractor; - //// printf("keypoints: %i\n",keypoints.size()); - //// cv::Mat out; - //// cv::drawKeypoints(model->frames[i]->rgb, keypoints_surf, out, cv::Scalar::all(255)); - //// cv::imshow("Kpts", out); - //// cv::waitKey(0); - // } - // show_refine = false;//refine show - // show_reg = false;//registration show - // show_scoring = false;//fuse scoring sho + //cv::ORB orb = cv::ORB(250);//,1.2f, 1, 3, 0,2, cv::ORB::HARRIS_SCORE, 31); + cv::ORB orb = cv::ORB(10);//,1.2f, 1, 3, 0,2, cv::ORB::HARRIS_SCORE, 31); + // for(unsigned int i = 0; i < model->frames.size(); i++){ + // std::vector keypoints; + // cv::Mat descriptors; + // //orb(model->frames[i]->rgb, model->modelmasks[i]->getMask(), keypoints, descriptors); + // orb(model->frames[i]->rgb, cv::Mat(), keypoints, descriptors); + // model->all_keypoints.push_back(keypoints); + // model->all_descriptors.push_back(descriptors); - MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); - massreg->timeout = 4*massreg_timeout; - massreg->viewer = viewer; - massreg->visualizationLvl = show_init_lvl; + // printf("keypoints: %i\n",keypoints.size()); - massreg->maskstep = std::max(1,int(0.25*double(model->frames.size()))); - massreg->nomaskstep = std::max(5,int(0.5+0.3*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massreg->nomask = true; - massreg->stopval = 0.0005; - massreg->setData(model->frames,model->modelmasks); - MassFusionResults mfr = massreg->getTransforms(model->relativeposes); + // cv::Mat descriptors_surf; + // std::vector keypoints_surf; + // detector.detect(model->frames[i]->rgb, keypoints_surf, model->modelmasks[i]->getMask() ); + // extractor.compute( model->frames[i]->rgb, keypoints_surf, descriptors_surf ); + // model->all_keypoints.push_back(keypoints_surf); + // model->all_descriptors.push_back(descriptors_surf); - /* - MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); - massreg->timeout = 4*massreg_timeout; - massreg->viewer = viewer; - massreg->visualizationLvl = show_init_lvl; + //// printf("keypoints: %i\n",keypoints.size()); + //// cv::Mat out; + //// cv::drawKeypoints(model->frames[i]->rgb, keypoints_surf, out, cv::Scalar::all(255)); + //// cv::imshow("Kpts", out); + //// cv::waitKey(0); + // } - massreg->maskstep = std::max(1,int(0.25*double(model->frames.size()))); - massreg->nomaskstep = std::max(5,int(0.5+0.5*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massreg->nomask = true; - massreg->stopval = 0.0001; + // show_refine = false;//refine show + // show_reg = false;//registration show + // show_scoring = false;//fuse scoring sho - //massreg->setData(model->frames,model->modelmasks); - massreg->addModelData(model, false); - std::vector p = model->submodels_relativeposes; - p.insert(p.end(), model->relativeposes.begin(), model->relativeposes.end()); + MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); + massreg->timeout = 4*massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = show_init_lvl; + massreg->maskstep = std::max(1,int(0.25*double(model->frames.size()))); + massreg->nomaskstep = std::max(5,int(0.5+0.3*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg->nomask = true; + massreg->stopval = 0.0005; - //MassFusionResults mfr = massreg->getTransforms(model->relativeposes); - MassFusionResults mfr = massreg->getTransforms(p); -*/ + massreg->setData(model->frames,model->modelmasks); + MassFusionResults mfr = massreg->getTransforms(model->relativeposes); - model->relativeposes.clear();// = mfr.poses; - model->relativeposes.insert( model->relativeposes.end(), mfr.poses.begin()+model->submodels.size(), mfr.poses.end()); - - vector > ocs = getOcclusionScores(model->relativeposes, model->frames, model->modelmasks, false, 1); - std::vector > scores = getScores(ocs); - std::vector partition = getPartition(scores,2,5,2); - - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - if(scores[i][j] >= 0){printf(" ");} - printf("%5.5f ",0.00001*scores[i][j]); - } - printf("\n"); - } - printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); - - - - model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); - printf("getSuperPoints done\n"); - vector cp; - vector cf; - vector cm; - for(unsigned int i = 0; i < model->relativeposes.size(); i++){ - cp.push_back(model->relativeposes[i]); - cf.push_back(model->frames[i]); - cm.push_back(model->modelmasks[i]); - } - //getGoodCompareFrames(cp,cf,cm); - - computeOcclusionAreas(cp,cf,cm); - exit(0); - model->rep_relativeposes = cp; - model->rep_frames = cf; - model->rep_modelmasks = cm; - model->save("latestModel"); - - // vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); - // std::vector > scores2 = getScores(ocs2); - - // double sumscore_aft = 0; - // for(unsigned int i = 0; i < scores2.size(); i++){ - // for(unsigned int j = 0; j < scores2.size(); j++){ - // sumscore_aft += scores2[i][j]; - // } - // } - - // printf("bef %f after %f\n",sumscore_bef,sumscore_aft); - // if(sumscore_aft >= sumscore_bef){ - // model->submodels_relativeposes = mfr.poses; - // } -} -void ModelUpdater::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, int type, bool debugg){ - bool * maskvec = modelmask->maskvec; - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); + /* + MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); + massreg->timeout = 4*massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = show_init_lvl; - unsigned int frameid = frame->id; + massreg->maskstep = std::max(1,int(0.25*double(model->frames.size()))); + massreg->nomaskstep = std::max(5,int(0.5+0.5*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massreg->nomask = true; + massreg->stopval = 0.0001; - Matrix4d ip = p.inverse(); + //massreg->setData(model->frames,model->modelmasks); + massreg->addModelData(model, false); + std::vector p = model->submodels_relativeposes; + p.insert(p.end(), model->relativeposes.begin(), model->relativeposes.end()); - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - float im00 = ip(0,0); float im01 = ip(0,1); float im02 = ip(0,2); float im03 = ip(0,3); - float im10 = ip(1,0); float im11 = ip(1,1); float im12 = ip(1,2); float im13 = ip(1,3); - float im20 = ip(2,0); float im21 = ip(2,1); float im22 = ip(2,2); float im23 = ip(2,3); + //MassFusionResults mfr = massreg->getTransforms(model->relativeposes); + MassFusionResults mfr = massreg->getTransforms(p); +*/ - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; + model->relativeposes.clear();// = mfr.poses; + model->relativeposes.insert( model->relativeposes.end(), mfr.poses.begin()+model->submodels.size(), mfr.poses.end()); - const unsigned int dst_width2 = camera->width - 2; - const unsigned int dst_height2 = camera->height - 2; - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float fx = camera->fx; - const float fy = camera->fy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - - //bool * isfused = new bool[width*height]; - std::vector isfused; - printf("wh: %i %i -> %i\n",width,height,width*height); - isfused.resize(width*height); - for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} - - - for(unsigned int ind = 0; ind < spvec.size();ind++){ - superpoint & sp = spvec[ind]; - - float src_x = sp.point(0); - float src_y = sp.point(1); - float src_z = sp.point(2); - - float src_nx = sp.normal(0); - float src_ny = sp.normal(1); - float src_nz = sp.normal(2); - - float src_r = sp.feature(0); - float src_g = sp.feature(1); - float src_b = sp.feature(2); - - double point_information = sp.point_information; - double feature_information = sp.feature_information; - - float tx = im00*src_x + im01*src_y + im02*src_z + im03; - float ty = im10*src_x + im11*src_y + im12*src_z + im13; - float tz = im20*src_x + im21*src_y + im22*src_z + im23; - - float itz = 1.0/tz; - float dst_w = fx*tx*itz + cx; - float dst_h = fy*ty*itz + cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * width + unsigned(dst_w+0.5); - - float dst_z = idepth*float(depthdata[dst_ind]); - float dst_nx = normalsdata[3*dst_ind+0]; - if(!isfused[dst_ind] && dst_z > 0 && dst_nx != 2){ - float dst_ny = normalsdata[3*dst_ind+1]; - float dst_nz = normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - cx) * dst_z * ifx; - float dst_y = (float(dst_h) - cy) * dst_z * ify; - - double dst_noise = dst_z * dst_z; - - double point_noise = 1.0/sqrt(point_information); - - float tnx = im00*src_nx + im01*src_ny + im02*src_nz; - float tny = im10*src_nx + im11*src_ny + im12*src_nz; - float tnz = im20*src_nx + im21*src_ny + im22*src_nz; - - double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - - double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); - d *= compare_mul; - - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - - if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE - float px = m00*dst_x + m01*dst_y + m02*dst_z + m03; - float py = m10*dst_x + m11*dst_y + m12*dst_z + m13; - float pz = m20*dst_x + m21*dst_y + m22*dst_z + m23; - - float pnx = m00*dst_nx + m01*dst_ny + m02*dst_nz; - float pny = m10*dst_nx + m11*dst_ny + m12*dst_nz; - float pnz = m20*dst_nx + m21*dst_ny + m22*dst_nz; - - float pb = rgbdata[3*dst_ind+0]; - float pg = rgbdata[3*dst_ind+1]; - float pr = rgbdata[3*dst_ind+2]; - - Vector3f pxyz (px ,py ,pz ); - Vector3f pnxyz (pnx,pny,pnz); - Vector3f prgb (pr ,pg ,pb ); - float weight = 1.0/(dst_noise*dst_noise); - superpoint sp2 = superpoint(pxyz,pnxyz,prgb, weight, weight, frameid); - sp.merge(sp2); - isfused[dst_ind] = true; - } - } - } - } - - int nr_fused = 0; - int nr_mask = 0; - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - nr_fused += isfused[ind]; - nr_mask += maskvec[ind] > 0; - } - } - - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - if(!isfused[ind] && maskvec[ind]){ - float z = idepth*float(depthdata[ind]); - float nx = normalsdata[3*ind+0]; - - if(z > 0 && nx != 2){ - float ny = normalsdata[3*ind+1]; - float nz = normalsdata[3*ind+2]; - - float x = (w - cx) * z * ifx; - float y = (h - cy) * z * ify; - - double noise = z * z; - - float px = m00*x + m01*y + m02*z + m03; - float py = m10*x + m11*y + m12*z + m13; - float pz = m20*x + m21*y + m22*z + m23; - float pnx = m00*nx + m01*ny + m02*nz; - float pny = m10*nx + m11*ny + m12*nz; - float pnz = m20*nx + m21*ny + m22*nz; - - float pb = rgbdata[3*ind+0]; - float pg = rgbdata[3*ind+1]; - float pr = rgbdata[3*ind+2]; - - Vector3f pxyz (px ,py ,pz ); - Vector3f pnxyz (pnx,pny,pnz); - Vector3f prgb (pr ,pg ,pb ); - float weight = 1.0/(noise*noise); - spvec.push_back(superpoint(pxyz,pnxyz,prgb, weight, weight, frameid)); - } - } - } - } - - //delete[] isfused; - - if(debugg){ - pcl::PointCloud::Ptr cloud = getPCLnormalcloud(spvec); - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - } -} + vector > ocs = getOcclusionScores(model->relativeposes, model->frames, model->modelmasks, false, 1); + std::vector > scores = getScores(ocs); + std::vector partition = getPartition(scores,2,5,2); -vector ModelUpdater::getSuperPoints(vector cp, vector cf, vector cm, int type, bool debugg){ - vector spvec; - for(unsigned int i = 0; i < cp.size(); i++){addSuperPoints(spvec, cp[i], cf[i], cm[i], type,debugg);} - return spvec; -} + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + if(scores[i][j] >= 0){printf(" ");} + printf("%5.5f ",0.00001*scores[i][j]); + } + printf("\n"); + } + printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); -double ModelUpdater::getCompareUtility(Matrix4d p, RGBDFrame* frame, ModelMask* mask, vector & cp, vector & cf, vector & cm){ - unsigned char * src_maskdata = (unsigned char *)(mask->mask.data); - unsigned char * src_rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(frame->depth.data); - float * src_normalsdata = (float *)(frame->normals.data); - - Camera * src_camera = frame->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - - std::vector test_x; - std::vector test_y; - std::vector test_z; - std::vector test_nx; - std::vector test_ny; - std::vector test_nz; - - std::vector & testw = mask->testw; - std::vector & testh = mask->testh; - - for(unsigned int ind = 0; ind < testw.size();ind++){ - unsigned int src_w = testw[ind]; - unsigned int src_h = testh[ind]; - - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] == 255){ - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; - - test_x.push_back(src_x); - test_y.push_back(src_y); - test_z.push_back(src_z); - test_nx.push_back(src_nx); - test_ny.push_back(src_ny); - test_nz.push_back(src_nz); - } - } - } - - bool debugg = false; - unsigned int nrdata_start = test_x.size(); - - for(unsigned int c = 0; c < cp.size();c++){ - RGBDFrame* dst = cf[c]; - if(dst == frame){continue;} - ModelMask* dst_modelmask = cm[c]; - unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); - unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); - float * dst_normalsdata = (float *)(dst->normals.data); - - Matrix4d rp = cp[c].inverse()*p; - - float m00 = rp(0,0); float m01 = rp(0,1); float m02 = rp(0,2); float m03 = rp(0,3); - float m10 = rp(1,0); float m11 = rp(1,1); float m12 = rp(1,2); float m13 = rp(1,3); - float m20 = rp(2,0); float m21 = rp(2,1); float m22 = rp(2,2); float m23 = rp(2,3); - - Camera * dst_camera = dst->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - if(debugg){ - viewer->removeAllPointClouds(); - - pcl::PointCloud::Ptr cloud2 = dst->getPCLcloud(); - viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud2"); - } - - for(unsigned int ind = 0; ind < test_x.size();ind++){ - float src_nx = test_nx[ind]; - float src_ny = test_ny[ind]; - float src_nz = test_nz[ind]; - - float src_x = test_x[ind]; - float src_y = test_y[ind]; - float src_z = test_z[ind]; - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - pcl::PointXYZRGBNormal point; - if(debugg){ - point.x = tx; - point.y = ty; - point.z = tz; - point.r = 255; - point.g = 0; - point.b = 0; - } - - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - double dst_noise = dst_z * dst_z; - double point_noise = src_z * src_z; - - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - - double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - - double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); - d *= compare_mul; - - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - - if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE - //if(d < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE - test_nx[ind] = test_nx.back(); test_nx.pop_back(); - test_ny[ind] = test_ny.back(); test_ny.pop_back(); - test_nz[ind] = test_nz.back(); test_nz.pop_back(); - test_x[ind] = test_x.back(); test_x.pop_back(); - test_y[ind] = test_y.back(); test_y.pop_back(); - test_z[ind] = test_z.back(); test_z.pop_back(); - ind--; - if(debugg){ - point.r = 0; - point.g = 255; - point.b = 0; - } - } - } - } - if(debugg){ - cloud->points.push_back(point); - } - } - - if(debugg){ - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - } - } - return double(test_x.size())/double(nrdata_start); -} -void ModelUpdater::getGoodCompareFrames(vector & cp, vector & cf, vector & cm){ - while(true){ - double worst = 1; - int worst_id = -1; - for(unsigned int i = 0; i < cp.size(); i++){ - double score = getCompareUtility(cp[i], cf[i], cm[i], cp, cf, cm); - //printf("%3.3i -> %f\n",i,score); - if(score < worst){ - worst = score; - worst_id = i; - } - } - - if(worst_id < 0){break;} - if(worst < 0.1){//worst frame can to 90% be represented by other frames - cp[worst_id] = cp.back(); - cp.pop_back(); - - cf[worst_id] = cf.back(); - cf.pop_back(); - - cm[worst_id] = cm.back(); - cm.pop_back(); - //printf("removing %i\n",worst_id); - }else{break;} - } -} -void ModelUpdater::refine(double reg,bool useFullMask, int visualization){ - //return ; - printf("void ModelUpdater::refine()\n"); - - printf("%s::%i\n",__FILE__,__LINE__); - vector > ocs = computeOcclusionScore(model->submodels,model->submodels_relativeposes,1,false); - printf("%s::%i\n",__FILE__,__LINE__); - std::vector > scores = getScores(ocs); - printf("%s::%i\n",__FILE__,__LINE__); - - double sumscore_bef = 0; - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - sumscore_bef += scores[i][j]; - } - } - - MassFusionResults mfr; - MassRegistrationPPR * massreg = new MassRegistrationPPR(reg); - massreg->timeout = massreg_timeout; - massreg->viewer = viewer; - massreg->visualizationLvl = 0; - massreg->maskstep = 4;//std::max(1,int(0.5+0.02*double(model->frames.size()))); - massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); - printf("maskstep: %i nomaskstep: %i\n",massreg->maskstep,massreg->nomaskstep); - massreg->nomask = true; - massreg->stopval = 0.001; - double step = model->submodels_relativeposes.size()*model->submodels_relativeposes.size(); - step *= 0.25; - step += 0.5; - massreg->maskstep = std::max(1.5,step); - massreg->addModelData(model); - mfr = massreg->getTransforms(model->submodels_relativeposes); - vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); - std::vector > scores2 = getScores(ocs2); - - double sumscore_aft = 0; - for(unsigned int i = 0; i < scores2.size(); i++){ - for(unsigned int j = 0; j < scores2.size(); j++){ - sumscore_aft += scores2[i][j]; - } - } - - printf("bef %f after %f\n",sumscore_bef,sumscore_aft); - if(sumscore_aft >= sumscore_bef){ - model->submodels_relativeposes = mfr.poses; - } + model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); + printf("getSuperPoints done\n"); + vector cp; + vector cf; + vector cm; + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + cp.push_back(model->relativeposes[i]); + cf.push_back(model->frames[i]); + cm.push_back(model->modelmasks[i]); + } + //getGoodCompareFrames(cp,cf,cm); + + computeOcclusionAreas(cp,cf,cm); + exit(0); + model->rep_relativeposes = cp; + model->rep_frames = cf; + model->rep_modelmasks = cm; + model->save("latestModel"); + + // vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); + // std::vector > scores2 = getScores(ocs2); + + // double sumscore_aft = 0; + // for(unsigned int i = 0; i < scores2.size(); i++){ + // for(unsigned int j = 0; j < scores2.size(); j++){ + // sumscore_aft += scores2[i][j]; + // } + // } + + // printf("bef %f after %f\n",sumscore_bef,sumscore_aft); + // if(sumscore_aft >= sumscore_bef){ + // model->submodels_relativeposes = mfr.poses; + // } } -void ModelUpdater::show(bool stop){ - //printf("void ModelUpdater::show()\n"); - pcl::PointCloud::Ptr cloud = model->getPCLcloud(1,false); - viewer->removeAllPointClouds(); - viewer->addPointCloud(cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - if(stop){ viewer->spin();} - else{ viewer->spinOnce();} - viewer->removeAllPointClouds(); -} +void ModelUpdater::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, int type, bool debugg){ + bool * maskvec = modelmask->maskvec; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); -//Backproject and prune occlusions -void ModelUpdater::pruneOcclusions(){} + unsigned int frameid = frame->id; -void ModelUpdater::computeResiduals(std::vector & residuals, std::vector & weights, - RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, - RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, - Eigen::Matrix4d p, bool debugg){ - - unsigned char * src_maskdata = (unsigned char *)(src_modelmask->mask.data); - unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(src->depth.data); - float * src_normalsdata = (float *)(src->normals.data); - - unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); - unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); - float * dst_normalsdata = (float *)(dst->normals.data); - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - Camera * src_camera = src->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - - Camera * dst_camera = dst->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - - std::vector & testw = src_modelmask->testw; - std::vector & testh = src_modelmask->testh; - - unsigned int test_nrdata = testw.size(); - for(unsigned int ind = 0; ind < test_nrdata;ind++){ - unsigned int src_w = testw[ind]; - unsigned int src_h = testh[ind]; - - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ - float z = src_idepth*float(src_depthdata[src_ind]); - float nx = src_normalsdata[3*src_ind+0]; - - if(z > 0 && nx != 2){ - float ny = src_normalsdata[3*src_ind+1]; - float nz = src_normalsdata[3*src_ind+2]; - - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; - - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - //float tnx = m00*nx + m01*ny + m02*nz; - //float tny = m10*nx + m11*ny + m12*nz; - float tnz = m20*nx + m21*ny + m22*nz; - - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - if(dst_z > 0){ - float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 - residuals.push_back(diff_z); - weights.push_back(1.0); - } - } - } - } - } -} + Matrix4d ip = p.inverse(); -void ModelUpdater::recomputeScores(){ - // printf("recomputeScores\n"); - std::vector > ocs = getOcclusionScores(model->relativeposes,model->frames,model->modelmasks,false); - model->scores = getScores(ocs); - - model->total_scores = 0; - for(unsigned int i = 0; i < model->scores.size(); i++){ - for(unsigned int j = 0; j < model->scores.size(); j++){ - model->total_scores += model->scores[i][j]; - } - } -} + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); -void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2){ - unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); - float * src_normalsdata = (float *)(frame1->normals.data); - - Camera * src_camera = frame1->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - - unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); - float * dst_normalsdata = (float *)(frame2->normals.data); - - Camera * dst_camera = frame2->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - bool debugg = false; - pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); - if(debugg){ - - src_cloud->points.resize(src_width*src_height); - for(unsigned int src_w = 0; src_w < src_width;src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; - - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; - point.r = 255; - point.g = 0; - point.b = 0; - src_cloud->points[src_ind] = point; - } - } - } - - dst_cloud->points.resize(dst_width*dst_height); - for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - int dst_ind = dst_h*dst_width+dst_w; - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - pcl::PointXYZRGBNormal point; - point.x = dst_x; - point.y = dst_y; - point.z = dst_z; - point.r = 0; - point.g = 0; - point.b = 255; - dst_cloud->points[dst_ind] = point; - } - } - } - } - - for(unsigned int src_w = 0; src_w < src_width;src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; - - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - double dst_noise = dst_z * dst_z; - double point_noise = src_z * src_z; - - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - - double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); - //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); - //d *= src_z;//compare_mul; - //double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - - if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE - overlaps1[src_ind] += weights2[dst_ind]; - overlaps2[dst_ind] += weights1[src_ind]; - - total1[src_ind] += weights2[dst_ind]; - total2[dst_ind] += weights1[src_ind]; - if(debugg){ - dst_cloud->points[dst_ind].r = 0; - dst_cloud->points[dst_ind].g = 255; - dst_cloud->points[dst_ind].b = 0; - src_cloud->points[src_ind].r = 0; - src_cloud->points[src_ind].g = 255; - src_cloud->points[src_ind].b = 0; - } - }else if(tz < dst_z){ - total1[src_ind] += weights2[dst_ind]; - total2[dst_ind] += weights1[src_ind]; - if(debugg){ - src_cloud->points[src_ind].r = 255; - src_cloud->points[src_ind].g = 0; - src_cloud->points[src_ind].b = 255; - } - } - } - } - } - } - } - - if(debugg){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); - viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); - viewer->spin(); - } -} + float im00 = ip(0,0); float im01 = ip(0,1); float im02 = ip(0,2); float im03 = ip(0,3); + float im10 = ip(1,0); float im11 = ip(1,1); float im12 = ip(1,2); float im13 = ip(1,3); + float im20 = ip(2,0); float im21 = ip(2,1); float im22 = ip(2,2); float im23 = ip(2,3); -double interpolate( double val, double y0, double x0, double y1, double x1 ) {return (val-x0)*(y1-y0)/(x1-x0) + y0;} -double base( double val ) { - if ( val <= -0.75 ) return 0; - else if ( val <= -0.25 ) return interpolate( val, 0.0, -0.75, 1.0, -0.25 ); - else if ( val <= 0.25 ) return 1.0; - else if ( val <= 0.75 ) return interpolate( val, 1.0, 0.25, 0.0, 0.75 ); - else return 0.0; -} -double red( double gray ) { return base( gray - 0.5 ); } -double green( double gray ) { return base( gray );} -double blue( double gray ) { return base( gray + 0.5 );} + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; -cv::Mat getImageFromArray(unsigned int width, unsigned int height, double * arr){ - double maxval = 0; - for(unsigned int i = 0; i < width*height; i++){maxval = std::max(maxval,arr[i]);} - maxval = 1; - - cv::Mat m; - m.create(height,width,CV_8UC3); - unsigned char * data = m.data; - for(unsigned int i = 0; i < width*height; i++){ - double d = arr[i]/maxval; - if(d < 0){ - data[3*i+0] = 255.0;//blue(d); - data[3*i+1] = 0;//green(d); - data[3*i+2] = 255.0;//red(d); - }else{ - data[3*i+0] = 255.0*d;//blue(d); - data[3*i+1] = 255.0*d;//green(d); - data[3*i+2] = 255.0*d;//red(d); - } - } - - cv::namedWindow( "getImageFromArray", cv::WINDOW_AUTOSIZE ); - cv::imshow( "getImageFromArray",m); - cv::waitKey(0); - - return m; -} + const unsigned int dst_width2 = camera->width - 2; + const unsigned int dst_height2 = camera->height - 2; -void ModelUpdater::computeOcclusionAreas(vector cp, vector cf, vector cm){ + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float fx = camera->fx; + const float fy = camera->fy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; - //double ** weights_old = new double*[]; - std::vector weights_old; - double ** wo = new double*[cf.size()]; - for(unsigned int i = 0; i < cf.size(); i++){ - unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; - wo[i] = new double[nr_pixels]; - for(unsigned int j = 0; j < nr_pixels; j++){ - wo[i][j] = 1; - } - } - - weights_old.push_back(wo); - - double ** overlaps = new double*[cf.size()]; - double ** total = new double*[cf.size()]; - for(unsigned int i = 0; i < cf.size(); i++){ - unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; - overlaps[i] = new double[nr_pixels]; - total[i] = new double[nr_pixels]; - } - - - - - - - - for(unsigned int iter = 0; iter < 10; iter++){ - for(unsigned int i = 0; i < cf.size(); i++){ - unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; - for(unsigned int j = 0; j < nr_pixels; j++){ - overlaps[i][j] = 0; - total[i][j] = 0; - } - } - printf("iter %i\n",iter); - for(unsigned int i = 0; i < cf.size(); i++){ - for(unsigned int j = 0; j < cf.size(); j++){ - if(i == j){continue;} - //weightedocclusioncounts - Eigen::Matrix4d p = cp[i].inverse() * cp[j]; - getAreaWeights(p.inverse(), cf[i], wo[i], overlaps[i], total[i], cf[j], wo[j], overlaps[j], total[j]); - } - } - - wo = new double*[cf.size()]; - for(unsigned int i = 0; i < cf.size(); i++){ - unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; - wo[i] = new double[nr_pixels]; - for(unsigned int j = 0; j < nr_pixels; j++){ - if(total[i][j] < 0.00001){ - wo[i][j] = 0.0; - }else{ - double fails = occlusion_penalty*(total[i][j]-overlaps[i][j]); - double others = overlaps[i][j]; - wo[i][j] = others/(fails+others); - } - } - } - weights_old.push_back(wo); - } - - for(unsigned int i = 0; i < cf.size(); i++){ - unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; - double sum = 0; - for(unsigned int j = 0; j < nr_pixels; j++){ - sum += wo[i][j]; - } - sum /= double(nr_pixels); - for(unsigned int j = 0; j < nr_pixels; j++){ - if(total[i][j] == 0){ - wo[i][j] = sum; - } - } - } - for(unsigned int i = 0; i < cf.size(); i++){ - for(unsigned int j = 0; j < weights_old.size(); j++){ - getImageFromArray(cf[i]->camera->width, cf[i]->camera->height, weights_old[j][i]); - } - } -} + //bool * isfused = new bool[width*height]; + std::vector isfused; + printf("wh: %i %i -> %i\n",width,height,width*height); + isfused.resize(width*height); + for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} -void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ - unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); - float * src_normalsdata = (float *)(frame1->normals.data); + for(unsigned int ind = 0; ind < spvec.size();ind++){ + superpoint & sp = spvec[ind]; - unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); - unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); + float src_x = sp.point(0); + float src_y = sp.point(1); + float src_z = sp.point(2); - Camera * src_camera = frame1->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - - unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); - float * dst_normalsdata = (float *)(frame2->normals.data); - unsigned char * dst_maskdata = (unsigned char *)(mask.data); - - Camera * dst_camera = frame2->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - //bool debugg = true; - pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); - if(debugg){ + float src_nx = sp.normal(0); + float src_ny = sp.normal(1); + float src_nz = sp.normal(2); + float src_r = sp.feature(0); + float src_g = sp.feature(1); + float src_b = sp.feature(2); -// cv::namedWindow( "src_det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_det_dilate", frame1->det_dilate); -// cv::namedWindow( "dst_det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "dst_det_dilate", frame2->det_dilate); -// cv::waitKey(0); + double point_information = sp.point_information; + double feature_information = sp.feature_information; - src_cloud->points.resize(src_width*src_height); - for(unsigned int src_w = 0; src_w < src_width;src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; + float tx = im00*src_x + im01*src_y + im02*src_z + im03; + float ty = im10*src_x + im11*src_y + im12*src_z + im13; + float tz = im20*src_x + im21*src_y + im22*src_z + im23; - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; + float itz = 1.0/tz; + float dst_w = fx*tx*itz + cx; + float dst_h = fy*ty*itz + cy; + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * width + unsigned(dst_w+0.5); - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float dst_z = idepth*float(depthdata[dst_ind]); + float dst_nx = normalsdata[3*dst_ind+0]; + if(!isfused[dst_ind] && dst_z > 0 && dst_nx != 2){ + float dst_ny = normalsdata[3*dst_ind+1]; + float dst_nz = normalsdata[3*dst_ind+2]; - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; - //point.r = 255*src_detdata[src_ind] != 0; -// point.g = 0; -// point.b = 0; + float dst_x = (float(dst_w) - cx) * dst_z * ifx; + float dst_y = (float(dst_h) - cy) * dst_z * ify; - point.r = src_detdata[src_ind]; - point.g = 0; - point.b = 0; + double dst_noise = dst_z * dst_z; - src_cloud->points[src_ind] = point; - } - } - } - - dst_cloud->points.resize(dst_width*dst_height); - for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - int dst_ind = dst_h*dst_width+dst_w; - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - pcl::PointXYZRGBNormal point; - point.x = dst_x; - point.y = dst_y; - point.z = dst_z; - point.r = 0; - point.g = 0; - point.b = dst_detdata[dst_ind]; - dst_cloud->points[dst_ind] = point; - } - } - } + double point_noise = 1.0/sqrt(point_information); + float tnx = im00*src_nx + im01*src_ny + im02*src_nz; + float tny = im10*src_nx + im11*src_ny + im12*src_nz; + float tnz = im20*src_nx + im21*src_ny + im22*src_nz; -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); -// viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); -// viewer->spin(); - } + double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - int informations = 0; + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; - for(unsigned int src_w = 0; src_w < src_width;src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; + if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE + float px = m00*dst_x + m01*dst_y + m02*dst_z + m03; + float py = m10*dst_x + m11*dst_y + m12*dst_z + m13; + float pz = m20*dst_x + m21*dst_y + m22*dst_z + m23; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - if(tz < 0){continue;} + float pnx = m00*dst_nx + m01*dst_ny + m02*dst_nz; + float pny = m10*dst_nx + m11*dst_ny + m12*dst_nz; + float pnz = m20*dst_nx + m21*dst_ny + m22*dst_nz; - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; + float pb = rgbdata[3*dst_ind+0]; + float pg = rgbdata[3*dst_ind+1]; + float pr = rgbdata[3*dst_ind+2]; - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + Vector3f pxyz (px ,py ,pz ); + Vector3f pnxyz (pnx,pny,pnz); + Vector3f prgb (pr ,pg ,pb ); + float weight = 1.0/(dst_noise*dst_noise); + superpoint sp2 = superpoint(pxyz,pnxyz,prgb, weight, weight, frameid); + sp.merge(sp2); + isfused[dst_ind] = true; + } + } + } + } - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; + int nr_fused = 0; + int nr_mask = 0; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + nr_fused += isfused[ind]; + nr_mask += maskvec[ind] > 0; + } + } - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(!isfused[ind] && maskvec[ind]){ + float z = idepth*float(depthdata[ind]); + float nx = normalsdata[3*ind+0]; - double dst_noise = dst_z * dst_z; - double point_noise = src_z * src_z; + if(z > 0 && nx != 2){ + float ny = normalsdata[3*ind+1]; + float nz = normalsdata[3*ind+2]; - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + float x = (w - cx) * z * ifx; + float y = (h - cy) * z * ify; - double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); - //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); - //d *= src_z;//compare_mul; - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + double noise = z * z; - if(fabs(d) <= 0.01){//If close, according noises, and angle of the surfaces similar: FUSE - if(surface_angle > 0.8 && dst_maskdata[dst_ind] > 0){ - if(interframe_connectionId.size() > 0){ - double p_same = 0.99; - double not_p_same = 1-p_same; - double weight = -log(not_p_same); + float px = m00*x + m01*y + m02*z + m03; + float py = m10*x + m11*y + m12*z + m13; + float pz = m20*x + m21*y + m22*z + m23; + float pnx = m00*nx + m01*ny + m02*nz; + float pny = m10*nx + m11*ny + m12*nz; + float pnz = m20*nx + m21*ny + m22*nz; - interframe_connectionId[src_ind+offset1].push_back(dst_ind+offset2); - interframe_connectionStrength[src_ind+offset1].push_back(weight); - } - if(dst_detdata[dst_ind] == 0 && src_detdata[src_ind] == 0){ - overlaps[src_ind]++; - if(debugg){ - informations++; - dst_cloud->points[dst_ind].r = 0; - dst_cloud->points[dst_ind].g = 255; - dst_cloud->points[dst_ind].b = 0; - src_cloud->points[src_ind].r = 0; - src_cloud->points[src_ind].g = 255; - src_cloud->points[src_ind].b = 0; - } - } - } - }else if(tz < dst_z && fabs(d) > 0.02 && dst_detdata[dst_ind] == 0 && src_detdata[src_ind] == 0){ + float pb = rgbdata[3*ind+0]; + float pg = rgbdata[3*ind+1]; + float pr = rgbdata[3*ind+2]; - occlusions[src_ind] ++; - if(debugg){ - informations++; - src_cloud->points[src_ind].r = 255; - src_cloud->points[src_ind].g = 0; - src_cloud->points[src_ind].b = 255; - } - } - } - } - } - } - } - - if(debugg){printf("informations: %i\n",informations);} - if(debugg && informations > 0){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); - viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); - viewer->spin(); - } + Vector3f pxyz (px ,py ,pz ); + Vector3f pnxyz (pnx,pny,pnz); + Vector3f prgb (pr ,pg ,pb ); + float weight = 1.0/(noise*noise); + spvec.push_back(superpoint(pxyz,pnxyz,prgb, weight, weight, frameid)); + } + } + } + } + + //delete[] isfused; + + if(debugg){ + pcl::PointCloud::Ptr cloud = getPCLnormalcloud(spvec); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } } -std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ - cv::Mat src = frame->rgb.clone(); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - const float idepth = frame->camera->idepth_scale; +vector ModelUpdater::getSuperPoints(vector cp, vector cf, vector cm, int type, bool debugg){ + vector spvec; + for(unsigned int i = 0; i < cp.size(); i++){addSuperPoints(spvec, cp[i], cf[i], cm[i], type,debugg);} + return spvec; +} - cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); +double ModelUpdater::getCompareUtility(Matrix4d p, RGBDFrame* frame, ModelMask* mask, vector & cp, vector & cf, vector & cm){ + unsigned char * src_maskdata = (unsigned char *)(mask->mask.data); + unsigned char * src_rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame->depth.data); + float * src_normalsdata = (float *)(frame->normals.data); + + Camera * src_camera = frame->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + std::vector test_x; + std::vector test_y; + std::vector test_z; + std::vector test_nx; + std::vector test_ny; + std::vector test_nz; + + std::vector & testw = mask->testw; + std::vector & testh = mask->testh; + + for(unsigned int ind = 0; ind < testw.size();ind++){ + unsigned int src_w = testw[ind]; + unsigned int src_h = testh[ind]; + + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] == 255){ + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + test_x.push_back(src_x); + test_y.push_back(src_y); + test_z.push_back(src_z); + test_nx.push_back(src_nx); + test_ny.push_back(src_ny); + test_nz.push_back(src_nz); + } + } + } - unsigned char * srcdata = (unsigned char *)src.data; - unsigned int width = src.cols; - unsigned int height = src.rows; + bool debugg = false; + unsigned int nrdata_start = test_x.size(); + + for(unsigned int c = 0; c < cp.size();c++){ + RGBDFrame* dst = cf[c]; + if(dst == frame){continue;} + ModelMask* dst_modelmask = cm[c]; + unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); + float * dst_normalsdata = (float *)(dst->normals.data); + + Matrix4d rp = cp[c].inverse()*p; + + float m00 = rp(0,0); float m01 = rp(0,1); float m02 = rp(0,2); float m03 = rp(0,3); + float m10 = rp(1,0); float m11 = rp(1,1); float m12 = rp(1,2); float m13 = rp(1,3); + float m20 = rp(2,0); float m21 = rp(2,1); float m22 = rp(2,2); float m23 = rp(2,3); + + Camera * dst_camera = dst->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + if(debugg){ + viewer->removeAllPointClouds(); + + pcl::PointCloud::Ptr cloud2 = dst->getPCLcloud(); + viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud2"); + } - std::vector dxc; - dxc.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0;} + for(unsigned int ind = 0; ind < test_x.size();ind++){ + float src_nx = test_nx[ind]; + float src_ny = test_ny[ind]; + float src_nz = test_nz[ind]; + + float src_x = test_x[ind]; + float src_y = test_y[ind]; + float src_z = test_z[ind]; + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + pcl::PointXYZRGBNormal point; + if(debugg){ + point.x = tx; + point.y = ty; + point.z = tz; + point.r = 255; + point.g = 0; + point.b = 0; + } - std::vector dyc; - dyc.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){dyc[i] = 0;} - std::vector src_dxdata; - src_dxdata.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){src_dxdata[i] = 0;} + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - std::vector src_dydata; - src_dydata.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){src_dydata[i] = 0;} + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; - std::vector maxima_dxdata; - maxima_dxdata.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){maxima_dxdata[i] = 0;} + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - std::vector maxima_dydata; - maxima_dydata.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){maxima_dydata[i] = 0;} + double dst_noise = dst_z * dst_z; + double point_noise = src_z * src_z; + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - src_dxdata[ind] = 0; - src_dydata[ind] = 0; - } - } + double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - unsigned int chans = 3; - for(unsigned int c = 0; c < chans;c++){ - for(unsigned int w = 1; w < width;w++){ - for(unsigned int h = 1; h < height;h++){ - int ind = h*width+w; - int dir = 1; - src_dxdata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-1)+c]) / 255.0)/3.0; + double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + d *= compare_mul; - dir = width; - src_dydata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-width)+c]) / 255.0)/3.0; + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE + //if(d < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + test_nx[ind] = test_nx.back(); test_nx.pop_back(); + test_ny[ind] = test_ny.back(); test_ny.pop_back(); + test_nz[ind] = test_nz.back(); test_nz.pop_back(); + test_x[ind] = test_x.back(); test_x.pop_back(); + test_y[ind] = test_y.back(); test_y.pop_back(); + test_z[ind] = test_z.back(); test_z.pop_back(); + ind--; + if(debugg){ + point.r = 0; + point.g = 255; + point.b = 0; + } + } + } + } + if(debugg){ + cloud->points.push_back(point); } } - } - - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - maxima_dxdata[ind] = (src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] > src_dxdata[ind+1]); - maxima_dydata[ind] = (src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] > src_dydata[ind+width]); + if(debugg){ + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); } } + return double(test_x.size())/double(nrdata_start); +} +void ModelUpdater::getGoodCompareFrames(vector & cp, vector & cf, vector & cm){ + while(true){ + double worst = 1; + int worst_id = -1; + for(unsigned int i = 0; i < cp.size(); i++){ + double score = getCompareUtility(cp[i], cf[i], cm[i], cp, cf, cm); + //printf("%3.3i -> %f\n",i,score); + if(score < worst){ + worst = score; + worst_id = i; + } + } + if(worst_id < 0){break;} + if(worst < 0.1){//worst frame can to 90% be represented by other frames + cp[worst_id] = cp.back(); + cp.pop_back(); + cf[worst_id] = cf.back(); + cf.pop_back(); - std::vector< std::vector > probs; + cm[worst_id] = cm.back(); + cm.pop_back(); + //printf("removing %i\n",worst_id); + }else{break;} + } +} - for(unsigned int c = 0; c < chans;c++){ - std::vector Xvec; - int dir; - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - dir = 1; - Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); +void ModelUpdater::refine(double reg,bool useFullMask, int visualization){ + //return ; + printf("void ModelUpdater::refine()\n"); + + printf("%s::%i\n",__FILE__,__LINE__); + vector > ocs = computeOcclusionScore(model->submodels,model->submodels_relativeposes,1,false); + printf("%s::%i\n",__FILE__,__LINE__); + std::vector > scores = getScores(ocs); + printf("%s::%i\n",__FILE__,__LINE__); + + double sumscore_bef = 0; + for(unsigned int i = 0; i < scores.size(); i++){ + for(unsigned int j = 0; j < scores.size(); j++){ + sumscore_bef += scores[i][j]; + } + } - dir = width; - Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - } + MassFusionResults mfr; + MassRegistrationPPR * massreg = new MassRegistrationPPR(reg); + massreg->timeout = massreg_timeout; + massreg->viewer = viewer; + massreg->visualizationLvl = 0; + massreg->maskstep = 4;//std::max(1,int(0.5+0.02*double(model->frames.size()))); + massreg->nomaskstep = std::max(1,int(0.5+1.0*double(model->frames.size()))); + printf("maskstep: %i nomaskstep: %i\n",massreg->maskstep,massreg->nomaskstep); + massreg->nomask = true; + massreg->stopval = 0.001; + double step = model->submodels_relativeposes.size()*model->submodels_relativeposes.size(); + step *= 0.25; + step += 0.5; + massreg->maskstep = std::max(1.5,step); + massreg->addModelData(model); + mfr = massreg->getTransforms(model->submodels_relativeposes); + vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); + std::vector > scores2 = getScores(ocs2); + + double sumscore_aft = 0; + for(unsigned int i = 0; i < scores2.size(); i++){ + for(unsigned int j = 0; j < scores2.size(); j++){ + sumscore_aft += scores2[i][j]; } + } - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); - for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + printf("bef %f after %f\n",sumscore_bef,sumscore_aft); + if(sumscore_aft >= sumscore_bef){ + model->submodels_relativeposes = mfr.poses; + } +} - double stdval = 0; - for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} - stdval = sqrt(stdval/double(Xvec.size())); +void ModelUpdater::show(bool stop){ + //printf("void ModelUpdater::show()\n"); + pcl::PointCloud::Ptr cloud = model->getPCLcloud(1,false); + viewer->removeAllPointClouds(); + viewer->addPointCloud(cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + if(stop){ viewer->spin();} + else{ viewer->spinOnce();} + viewer->removeAllPointClouds(); +} - DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); - func->zeromean = true; - func->maxp = 0.9999; - func->startreg = 0.5; - func->debugg_print = false; - func->bidir = true; - func->maxd = 256.0; - func->histogram_size = 256; - func->fixed_histogram_size = true; - func->startmaxd = func->maxd; - func->starthistogram_size = func->histogram_size; - func->blurval = 0.005; - func->stdval2 = stdval; - func->maxnoise = stdval; - func->reset(); - func->computeModel(X); +//Backproject and prune occlusions +void ModelUpdater::pruneOcclusions(){} - std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} - std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} - for(unsigned int w = 1; w < width;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; +void ModelUpdater::computeResiduals(std::vector & residuals, std::vector & weights, + RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, + RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, + Eigen::Matrix4d p, bool debugg){ + + unsigned char * src_maskdata = (unsigned char *)(src_modelmask->mask.data); + unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(src->depth.data); + float * src_normalsdata = (float *)(src->normals.data); + + unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); + float * dst_normalsdata = (float *)(dst->normals.data); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + Camera * src_camera = src->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + Camera * dst_camera = dst->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + std::vector & testw = src_modelmask->testw; + std::vector & testh = src_modelmask->testh; + + unsigned int test_nrdata = testw.size(); + for(unsigned int ind = 0; ind < test_nrdata;ind++){ + unsigned int src_w = testw[ind]; + unsigned int src_h = testh[ind]; + + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ + float z = src_idepth*float(src_depthdata[src_ind]); + float nx = src_normalsdata[3*src_ind+0]; + + if(z > 0 && nx != 2){ + float ny = src_normalsdata[3*src_ind+1]; + float nz = src_normalsdata[3*src_ind+2]; + + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + //float tnx = m00*nx + m01*ny + m02*nz; + //float tny = m10*nx + m11*ny + m12*nz; + float tnz = m20*nx + m21*ny + m22*nz; + + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + if(dst_z > 0){ + float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 + residuals.push_back(diff_z); + weights.push_back(1.0); + } + } + } + } + } +} - dir = 1; - dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); +void ModelUpdater::recomputeScores(){ + // printf("recomputeScores\n"); + std::vector > ocs = getOcclusionScores(model->relativeposes,model->frames,model->modelmasks,false); + model->scores = getScores(ocs); + + model->total_scores = 0; + for(unsigned int i = 0; i < model->scores.size(); i++){ + for(unsigned int j = 0; j < model->scores.size(); j++){ + model->total_scores += model->scores[i][j]; + } + } +} - dir = width; - dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); +void ModelUpdater::getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2){ + unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); + float * src_normalsdata = (float *)(frame1->normals.data); + + Camera * src_camera = frame1->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); + float * dst_normalsdata = (float *)(frame2->normals.data); + + Camera * dst_camera = frame2->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + bool debugg = false; + pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); + if(debugg){ + + src_cloud->points.resize(src_width*src_height); + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + point.r = 255; + point.g = 0; + point.b = 0; + src_cloud->points[src_ind] = point; + } } } - delete func; - probs.push_back(dx); - probs.push_back(dy); + dst_cloud->points.resize(dst_width*dst_height); + for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + pcl::PointXYZRGBNormal point; + point.x = dst_x; + point.y = dst_y; + point.z = dst_z; + point.r = 0; + point.g = 0; + point.b = 255; + dst_cloud->points[dst_ind] = point; + } + } + } } - { - std::vector Xvec; - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} - } - if(h > 1){ - int dir = -width; - int other2 = ind+2*dir; - int other = ind+dir; + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + double dst_noise = dst_z * dst_z; + double point_noise = src_z * src_z; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); + //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); + //d *= src_z;//compare_mul; + //double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(fabs(d) < 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + overlaps1[src_ind] += weights2[dst_ind]; + overlaps2[dst_ind] += weights1[src_ind]; + + total1[src_ind] += weights2[dst_ind]; + total2[dst_ind] += weights1[src_ind]; + if(debugg){ + dst_cloud->points[dst_ind].r = 0; + dst_cloud->points[dst_ind].g = 255; + dst_cloud->points[dst_ind].b = 0; + src_cloud->points[src_ind].r = 0; + src_cloud->points[src_ind].g = 255; + src_cloud->points[src_ind].b = 0; + } + }else if(tz < dst_z){ + total1[src_ind] += weights2[dst_ind]; + total2[dst_ind] += weights1[src_ind]; + if(debugg){ + src_cloud->points[src_ind].r = 255; + src_cloud->points[src_ind].g = 0; + src_cloud->points[src_ind].b = 255; + } + } + } } } } + } - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); - for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + if(debugg){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); + viewer->spin(); + } +} - double stdval = 0; - for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} - stdval = sqrt(stdval/double(Xvec.size())); +double interpolate( double val, double y0, double x0, double y1, double x1 ) {return (val-x0)*(y1-y0)/(x1-x0) + y0;} +double base( double val ) { + if ( val <= -0.75 ) return 0; + else if ( val <= -0.25 ) return interpolate( val, 0.0, -0.75, 1.0, -0.25 ); + else if ( val <= 0.25 ) return 1.0; + else if ( val <= 0.75 ) return interpolate( val, 1.0, 0.25, 0.0, 0.75 ); + else return 0.0; +} +double red( double gray ) { return base( gray - 0.5 ); } +double green( double gray ) { return base( gray );} +double blue( double gray ) { return base( gray + 0.5 );} - DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); - funcZ->zeromean = true; - funcZ->startreg = 0.002; - funcZ->debugg_print = false; - funcZ->bidir = true; - funcZ->maxp = 0.999999; - funcZ->maxd = 0.1; - funcZ->histogram_size = 100; - funcZ->fixed_histogram_size = true; - funcZ->startmaxd = funcZ->maxd; - funcZ->starthistogram_size = funcZ->histogram_size; - funcZ->blurval = 0.5; - funcZ->stdval2 = stdval; - funcZ->maxnoise = stdval; - funcZ->reset(); - funcZ->computeModel(X); +cv::Mat getImageFromArray(unsigned int width, unsigned int height, double * arr){ + double maxval = 0; + for(unsigned int i = 0; i < width*height; i++){maxval = std::max(maxval,arr[i]);} + maxval = 1; + + cv::Mat m; + m.create(height,width,CV_8UC3); + unsigned char * data = m.data; + for(unsigned int i = 0; i < width*height; i++){ + double d = arr[i]/maxval; + if(d < 0){ + data[3*i+0] = 255.0;//blue(d); + data[3*i+1] = 0;//green(d); + data[3*i+2] = 255.0;//red(d); + }else{ + data[3*i+0] = 255.0*d;//blue(d); + data[3*i+1] = 255.0*d;//green(d); + data[3*i+2] = 255.0*d;//red(d); + } + } - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); + cv::namedWindow( "getImageFromArray", cv::WINDOW_AUTOSIZE ); + cv::imshow( "getImageFromArray",m); + cv::waitKey(0); - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; + return m; +} - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; +void ModelUpdater::computeOcclusionAreas(vector cp, vector cf, vector cm){ - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} - } + //double ** weights_old = new double*[]; + std::vector weights_old; + double ** wo = new double*[cf.size()]; + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + wo[i] = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + wo[i][j] = 1; + } + } - if(h > 1){ - int dir = -width; - int other2 = ind+2*dir; - int other = ind+dir; + weights_old.push_back(wo); - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; + double ** overlaps = new double*[cf.size()]; + double ** total = new double*[cf.size()]; + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + overlaps[i] = new double[nr_pixels]; + total[i] = new double[nr_pixels]; + } - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + + + + + + + for(unsigned int iter = 0; iter < 10; iter++){ + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + for(unsigned int j = 0; j < nr_pixels; j++){ + overlaps[i][j] = 0; + total[i][j] = 0; + } + } + printf("iter %i\n",iter); + for(unsigned int i = 0; i < cf.size(); i++){ + for(unsigned int j = 0; j < cf.size(); j++){ + if(i == j){continue;} + //weightedocclusioncounts + Eigen::Matrix4d p = cp[i].inverse() * cp[j]; + getAreaWeights(p.inverse(), cf[i], wo[i], overlaps[i], total[i], cf[j], wo[j], overlaps[j], total[j]); + } + } + + wo = new double*[cf.size()]; + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + wo[i] = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + if(total[i][j] < 0.00001){ + wo[i][j] = 0.0; + }else{ + double fails = occlusion_penalty*(total[i][j]-overlaps[i][j]); + double others = overlaps[i][j]; + wo[i][j] = others/(fails+others); } } } + weights_old.push_back(wo); + } - std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} - std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + double sum = 0; + for(unsigned int j = 0; j < nr_pixels; j++){ + sum += wo[i][j]; + } + sum /= double(nr_pixels); + for(unsigned int j = 0; j < nr_pixels; j++){ + if(total[i][j] == 0){ + wo[i][j] = sum; + } + } + } + for(unsigned int i = 0; i < cf.size(); i++){ + for(unsigned int j = 0; j < weights_old.size(); j++){ + getImageFromArray(cf[i]->camera->width, cf[i]->camera->height, weights_old[j][i]); + } + } +} - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; +void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ + unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); + float * src_normalsdata = (float *)(frame1->normals.data); + unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); + unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); + Camera * src_camera = frame1->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); + float * dst_normalsdata = (float *)(frame2->normals.data); + unsigned char * dst_maskdata = (unsigned char *)(mask.data); + + Camera * dst_camera = frame2->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + //bool debugg = true; + pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); + if(debugg){ - float dz = fabs(z-z2); - if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} - if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} - } - if(h > 1){ - int dir = -width; +// cv::namedWindow( "src_det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_det_dilate", frame1->det_dilate); +// cv::namedWindow( "dst_det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "dst_det_dilate", frame2->det_dilate); +// cv::waitKey(0); - int other2 = ind+2*dir; - int other = ind+dir; + src_cloud->points.resize(src_width*src_height); + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; - float dz = fabs(z-z2); - if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} - if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + //point.r = 255*src_detdata[src_ind] != 0; +// point.g = 0; +// point.b = 0; + + point.r = src_detdata[src_ind]; + point.g = 0; + point.b = 0; + + src_cloud->points[src_ind] = point; } } } - delete funcZ; - probs.push_back(dx); - probs.push_back(dy); + dst_cloud->points.resize(dst_width*dst_height); + for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + pcl::PointXYZRGBNormal point; + point.x = dst_x; + point.y = dst_y; + point.z = dst_z; + point.r = 0; + point.g = 0; + point.b = dst_detdata[dst_ind]; + dst_cloud->points[dst_ind] = point; + } + } + } + + +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); +// viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); +// viewer->spin(); } + int informations = 0; - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; - float probX = 0; - float probY = 0; + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; - if(w > 0 && w < width-1){ - float ax = 0.5; - float bx = 0.5; - for(unsigned int p = 0; p < probs.size()-2; p+=2){ - float pr = probs[p][ind]; - ax *= pr; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + if(tz < 0){continue;} + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + double dst_noise = dst_z * dst_z; + double point_noise = src_z * src_z; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / (src_z*src_z + dst_z*dst_z); + //double compare_mul = 1/sqrt(dst_noise*dst_noise + point_noise*point_noise); + //d *= src_z;//compare_mul; + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(fabs(d) <= 0.01){//If close, according noises, and angle of the surfaces similar: FUSE + if(surface_angle > 0.8 && dst_maskdata[dst_ind] > 0){ + if(interframe_connectionId.size() > 0){ + double p_same = 0.99; + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + + interframe_connectionId[src_ind+offset1].push_back(dst_ind+offset2); + interframe_connectionStrength[src_ind+offset1].push_back(weight); + } + if(dst_detdata[dst_ind] == 0 && src_detdata[src_ind] == 0){ + overlaps[src_ind]++; + if(debugg){ + informations++; + dst_cloud->points[dst_ind].r = 0; + dst_cloud->points[dst_ind].g = 255; + dst_cloud->points[dst_ind].b = 0; + src_cloud->points[src_ind].r = 0; + src_cloud->points[src_ind].g = 255; + src_cloud->points[src_ind].b = 0; + } + } + } + }else if(tz < dst_z && fabs(d) > 0.02 && dst_detdata[dst_ind] == 0 && src_detdata[src_ind] == 0){ + + occlusions[src_ind] ++; + if(debugg){ + informations++; + src_cloud->points[src_ind].r = 255; + src_cloud->points[src_ind].g = 0; + src_cloud->points[src_ind].b = 255; + } + } + } + } + } + } + } + + if(debugg){printf("informations: %i\n",informations);} + if(debugg && informations > 0){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); + viewer->spin(); + } +} + +std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ + cv::Mat src = frame->rgb.clone(); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + const float idepth = frame->camera->idepth_scale; + + cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); + + unsigned char * srcdata = (unsigned char *)src.data; + unsigned int width = src.cols; + unsigned int height = src.rows; + + std::vector dxc; + dxc.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0;} + + std::vector dyc; + dyc.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){dyc[i] = 0;} + + std::vector src_dxdata; + src_dxdata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){src_dxdata[i] = 0;} + + std::vector src_dydata; + src_dydata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){src_dydata[i] = 0;} + + std::vector maxima_dxdata; + maxima_dxdata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){maxima_dxdata[i] = 0;} + + std::vector maxima_dydata; + maxima_dydata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){maxima_dydata[i] = 0;} + + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + src_dxdata[ind] = 0; + src_dydata[ind] = 0; + } + } + + unsigned int chans = 3; + for(unsigned int c = 0; c < chans;c++){ + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 1; h < height;h++){ + int ind = h*width+w; + int dir = 1; + src_dxdata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-1)+c]) / 255.0)/3.0; + + dir = width; + src_dydata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-width)+c]) / 255.0)/3.0; + } + } + + } + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + maxima_dxdata[ind] = (src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] > src_dxdata[ind+1]); + maxima_dydata[ind] = (src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] > src_dydata[ind+width]); + } + } + + + + + std::vector< std::vector > probs; + + for(unsigned int c = 0; c < chans;c++){ + std::vector Xvec; + int dir; + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + dir = 1; + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = width; + Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + } + } + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + func->zeromean = true; + func->maxp = 0.9999; + func->startreg = 0.5; + func->debugg_print = false; + func->bidir = true; + func->maxd = 256.0; + func->histogram_size = 256; + func->fixed_histogram_size = true; + func->startmaxd = func->maxd; + func->starthistogram_size = func->histogram_size; + func->blurval = 0.005; + func->stdval2 = stdval; + func->maxnoise = stdval; + func->reset(); + func->computeModel(X); + + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + + dir = 1; + dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + + dir = width; + dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + } + } + delete func; + + probs.push_back(dx); + probs.push_back(dy); + } + + { + std::vector Xvec; + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + } + } + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); + funcZ->zeromean = true; + funcZ->startreg = 0.002; + funcZ->debugg_print = false; + funcZ->bidir = true; + funcZ->maxp = 0.999999; + funcZ->maxd = 0.1; + funcZ->histogram_size = 100; + funcZ->fixed_histogram_size = true; + funcZ->startmaxd = funcZ->maxd; + funcZ->starthistogram_size = funcZ->histogram_size; + funcZ->blurval = 0.5; + funcZ->stdval2 = stdval; + funcZ->maxnoise = stdval; + funcZ->reset(); + funcZ->computeModel(X); + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + z2 = 2*z2-z3; + + if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + } + } + } + + std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} + std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + int dir = -1; + int other2 = ind+2*dir; + int other = ind+dir; + + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + + float dz = fabs(z-z2); + if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} + if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + } + + if(h > 1){ + int dir = -width; + + int other2 = ind+2*dir; + int other = ind+dir; + + float z3 = idepth*float(depthdata[other2]); + float z2 = idepth*float(depthdata[other]); + + float dz = fabs(z-z2); + if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} + if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + } + } + } + + delete funcZ; + probs.push_back(dx); + probs.push_back(dy); + } + + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + + float probX = 0; + float probY = 0; + + if(w > 0 && w < width-1){ + float ax = 0.5; + float bx = 0.5; + for(unsigned int p = 0; p < probs.size()-2; p+=2){ + float pr = probs[p][ind]; + ax *= pr; bx *= 1.0-pr; } float px = ax/(ax+bx); @@ -2087,1954 +2087,1802 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int probX = 1-current; } - if(h > 0 && h < height-1){ - float ay = 0.5; - float by = 0.5; - for(unsigned int p = 1; p < probs.size()-2; p+=2){ - float pr = probs[p][ind]; - ay *= pr; - by *= 1.0-pr; + if(h > 0 && h < height-1){ + float ay = 0.5; + float by = 0.5; + for(unsigned int p = 1; p < probs.size()-2; p+=2){ + float pr = probs[p][ind]; + ay *= pr; + by *= 1.0-pr; + } + float py = ay/(ay+by); + float current = 0; + if(!frame->det_dilate.data[ind]){ current = (1-py) * float(maxima_dydata[ind]);} + else{ current = std::max(float(1-probs[probs.size()-1][ind]),0.99f*(1-py) * float(maxima_dydata[ind]));} + probY = 1-current; + } + + dxc[ind] = std::min(probX,probY); + dyc[ind] = std::min(probX,probY); + } + } + + + std::vector< std::vector > probs2; + probs2.push_back(dxc); + probs2.push_back(dyc); + + cv::Mat edges; + edges.create(height,width,CV_32FC3); + float * edgesdata = (float *)edges.data; + + for(unsigned int i = 0; i < width*height;i++){ + edgesdata[3*i+0] = 0; + edgesdata[3*i+1] = dxc[i]; + edgesdata[3*i+2] = dyc[i]; + } + +// cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); +// cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); +// cv::waitKey(0); + + return probs2; +} + +std::vector doInference(std::vector & prior, std::vector< std::vector > & connectionId, std::vector< std::vector > & connectionStrength){ + double start_inf = getTime(); + + unsigned int nr_data = prior.size(); + unsigned int nr_edges = 0; + for(unsigned int j = 0; j < nr_data;j++){ + nr_edges += connectionId[j].size(); + } + + gc::Graph * g = new gc::Graph(nr_data,nr_edges); + for(unsigned int i = 0; i < nr_data;i++){ + g -> add_node(); + double p_fg = prior[i]; + if(p_fg < 0){ + g -> add_tweights( i, 0, 0 ); + continue; + } + double p_bg = 1-p_fg; + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + g -> add_tweights( i, weightFG, weightBG ); + } + + for(unsigned int i = 0; i < nr_data;i++){ + for(unsigned int j = 0; j < connectionId[i].size();j++){ + double weight = connectionStrength[i][j]; + g -> add_edge( i, connectionId[i][j], weight, weight ); + //try{g -> add_edge( i, connectionId[i][j], weight, weight );} + //catch(std::exception ex){printf("%i %i -> weight: %20.20f\n",i,connectionId[i],weight);exit(0);} + } + } + + g -> maxflow(); + + std::vector labels; + labels.resize(nr_data); + for(unsigned int i = 0; i < nr_data;i++){labels[i] = g->what_segment(i);} + delete g; + + double end_inf = getTime(); + printf("nr data: %i nr edges: %i inference time: %10.10fs\n",nr_data,nr_edges,end_inf-start_inf); + + return labels; +} + +void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, double debugg){ + + unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); + float * src_normalsdata = (float *)(frame1->normals.data); + + + unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); + unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); + + Camera * src_camera = frame1->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); + float * dst_normalsdata = (float *)(frame2->normals.data); + + Camera * dst_camera = frame2->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + //bool debugg = true; + pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); + if(debugg){ + + src_cloud->points.resize(src_width*src_height); + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + point.r = 255; + point.g = 0; + point.b = 0; + src_cloud->points[src_ind] = point; + } + } + } + + dst_cloud->points.resize(dst_width*dst_height); + for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + pcl::PointXYZRGBNormal point; + point.x = dst_x; + point.y = dst_y; + point.z = dst_z; + point.r = 0; + point.g = 0; + point.b = 255; + dst_cloud->points[dst_ind] = point; + } + } + } + } + + std::vector inds; + std::vector distances; + + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + + + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + if(tz <= 0){continue;} + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + double dst_noise = dst_z * dst_z; + double point_noise = src_z * src_z; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / sqrt(dst_noise*dst_noise + point_noise*point_noise); + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + if(dst_detdata[dst_ind] != 0){continue;} + if(src_detdata[src_ind] != 0){continue;} + + if(fabs(d) < 0.02){//If close, according noises, and angle of the surfaces similar: FUSE + if(surface_angle > 0.9){ + overlaps[src_ind] = std::max(overlaps[src_ind],0.9); + // overlaps[src_ind]++; + // if(debugg){ + //// dst_cloud->points[dst_ind].r = 0; + //// dst_cloud->points[dst_ind].g = 255; + //// dst_cloud->points[dst_ind].b = 0; + // src_cloud->points[src_ind].r = 0; + // src_cloud->points[src_ind].g = 255; + // src_cloud->points[src_ind].b = 0; + // } + } + }else if(tz < dst_z){ + occlusions[src_ind] = std::max(occlusions[src_ind],0.9); + if(debugg){ + src_cloud->points[src_ind].r = 255; + src_cloud->points[src_ind].g = 0; + src_cloud->points[src_ind].b = 255; + } + } + } } - float py = ay/(ay+by); - float current = 0; - if(!frame->det_dilate.data[ind]){ current = (1-py) * float(maxima_dydata[ind]);} - else{ current = std::max(float(1-probs[probs.size()-1][ind]),0.99f*(1-py) * float(maxima_dydata[ind]));} - probY = 1-current; } + } + } - dxc[ind] = std::min(probX,probY); - dyc[ind] = std::min(probX,probY); + if(debugg){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); + viewer->spin(); + } +} + +void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ + int tot_nr_pixels = 0; + std::vector offsets; + + for(unsigned int i = 0; i < frames.size(); i++){ + offsets.push_back(tot_nr_pixels); + unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; + tot_nr_pixels += nr_pixels; + } + printf("tot_nr_pixels: %i\n",tot_nr_pixels); + + + int current_point = 0; + double * priors = new double[3*tot_nr_pixels]; + double * priors_weight = new double[3*tot_nr_pixels]; + double * estimates = new double[3*tot_nr_pixels]; + double * points = new double[3*tot_nr_pixels]; + double * noise = new double[3*tot_nr_pixels]; + double * colors = new double[3*tot_nr_pixels]; + double * normals = new double[3*tot_nr_pixels]; + + std::vector< std::vector< std::vector > > pixel_weights; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + + for(unsigned int i = 0; i < frames.size(); i++){ + int offset = offsets[i]; + RGBDFrame * frame = frames[i]; + std::vector< std::vector > probs = getImageProbs(frame,9); + pixel_weights.push_back(probs); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + float * normalsdata = (float *)(frame->normals.data); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + + unsigned int nr_pixels = width * height; + double * current_overlaps = new double[nr_pixels]; + double * current_occlusions = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + current_overlaps[j] = 0; + current_occlusions[j] = 0; + } + + for(unsigned int j = 0; j < frames.size(); j++){ + if(i == j){continue;} + Eigen::Matrix4d p = poses[i].inverse() * poses[j]; + getDynamicWeights(p.inverse(),frames[i], current_overlaps, current_occlusions, frames[j],false); + } + + + double * bg_overlaps = new double[nr_pixels]; + double * bg_occlusions = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + bg_overlaps[j] = 0; + bg_occlusions[j] = 0; + } + + for(unsigned int j = 0; j < bgcf.size(); j++){ + Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; + getDynamicWeights(p.inverse(),frames[i], bg_overlaps, bg_occlusions, bgcf[j],false); + } + + Eigen::Matrix4d p = poses[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + + float minprob = 0.01; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + float p_moving = current_occlusions[ind]; + float p_dynamic = std::max((bg_occlusions[ind]-p_moving)*current_overlaps[ind],0.0); + p_dynamic += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); + p_moving += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); + + float p_static = std::max(bg_overlaps[ind]-p_moving-p_dynamic,0.0); + + float leftover = 1-p_moving-p_dynamic-p_static; + float notMoving = current_overlaps[ind]; + + float bias = 0.0001; + float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; + float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; + float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; + + float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover) + minprob; + float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover) + minprob; + float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover) + 2.0*minprob; + + pcl::PointXYZRGBNormal point; + point.x = m00*x + m01*y + m02*z + m03; + point.y = m10*x + m11*y + m12*z + m13; + point.z = m20*x + m21*y + m22*z + m23; + point.r = p_moving_tot*255.0; + point.g = p_dynamic_tot*255.0; + point.b = p_static_tot*255.0; + cloud->points.push_back(point); + /* + priors[3*current_point+0] = p_moving_tot; + priors[3*current_point+1] = p_dynamic_tot; + priors[3*current_point+2] = p_static_tot; + priors_weight[3*current_point+0] = -log(1-p_moving_tot); + priors_weight[3*current_point+1] = -log(1-p_dynamic_tot); + priors_weight[3*current_point+2] = -log(1-p_static_tot); + current_point++; + */ + } } + + delete[] current_occlusions; + delete[] current_overlaps; + delete[] bg_occlusions; + delete[] bg_overlaps; } + if(debugg){ + for(unsigned int i = 0; i < current_point; i++){ + cloud->points[i].r = estimates[3*i+0]*255.0; + cloud->points[i].g = estimates[3*i+1]*255.0; + cloud->points[i].b = estimates[3*i+2]*255.0; + } - std::vector< std::vector > probs2; - probs2.push_back(dxc); - probs2.push_back(dyc); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } - cv::Mat edges; - edges.create(height,width,CV_32FC3); - float * edgesdata = (float *)edges.data; + std::vector prior; + prior.resize(current_point); + for(unsigned int i = 0; i < current_point; i++){ + prior[i] = estimates[3*i+0]; +// cloud->points[i].g = estimates[3*i+1]*255.0; +// cloud->points[i].b = estimates[3*i+2]*255.0; + } - for(unsigned int i = 0; i < width*height;i++){ - edgesdata[3*i+0] = 0; - edgesdata[3*i+1] = dxc[i]; - edgesdata[3*i+2] = dyc[i]; + if(debugg){ + for(unsigned int i = 0; i < current_point; i++){ + cloud->points[i].r = estimates[3*i+0]*255.0; + cloud->points[i].g = (1-estimates[3*i+0])*255.0; + cloud->points[i].b = 0; + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); } +// +// std::vector< std::vector > frame_connectionId; +// std::vector< std::vector > frame_connectionStrength; + +// frame_prior.resize(nr_pixels); +// frame_connectionId.resize(nr_pixels); +// frame_connectionStrength.resize(nr_pixels); + +// for(unsigned int i = 0; i < interframe_connectionId.size(); i++){ +// for(unsigned int j = 0; j < interframe_connectionId[i].size(); j++){ +// connectionId[i].push_back(interframe_connectionId[i][j]); +// connectionStrength[i].push_back(interframe_connectionStrength[i][j]); +// } +// } +// std::vector improvedglobal_labels = doInference(prior,connectionId,connectionStrength); + + delete[] estimates; + delete[] priors_weight; + delete[] priors; + delete[] points; + delete[] colors; + delete[] normals; + + exit(0); +} -// cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); -// cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); -// cv::waitKey(0); - return probs2; -} +vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg){ +// +std::vector newmasks; +//for(double test = 0.99; test > 0.00000001; test *= 0.1) +{ +// double maxprob_same = 1-test;//0.99; + + double maxprob_same = 0.999999; + + double maxprob = 0.7; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud1 (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); + + pcl::PointCloud::Ptr bgcloud (new pcl::PointCloud); + + int tot_nr_pixels = 0; + std::vector offsets; + + for(unsigned int i = 0; i < frames.size(); i++){ + offsets.push_back(tot_nr_pixels); + unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; + tot_nr_pixels += nr_pixels; + } + + //Graph... + std::vector prior; + std::vector< std::vector > connectionId; + std::vector< std::vector > connectionStrength; + std::vector< std::vector > interframe_connectionId; + std::vector< std::vector > interframe_connectionStrength; + + prior.resize(tot_nr_pixels); + connectionId.resize(tot_nr_pixels); + connectionStrength.resize(tot_nr_pixels); + interframe_connectionId.resize(tot_nr_pixels); + interframe_connectionStrength.resize(tot_nr_pixels); + + + + for(unsigned int i = 0; i < frames.size(); i++){ + printf("frame: %i\n",i); + int offset = offsets[i]; + RGBDFrame * frame = frames[i]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned char * detdata = (unsigned char *)(frame->det_dilate.data); + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + + unsigned int nr_pixels = frame->camera->width * frame->camera->height; + double * overlaps = new double[nr_pixels]; + double * occlusions = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + overlaps[j] = 0; + occlusions[j] = 0; + } + + std::vector< std::vector > interframe_connectionId_tmp; + std::vector< std::vector > interframe_connectionStrength_tmp; + + for(unsigned int j = 0; j < bgcp.size(); j++){ + if(frame == bgcf[j]){continue;} + Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); + } + + for(unsigned int j = 0; j < nr_pixels; j++){ + occlusions[j] = 0; + } + printf("CHECK AGAINST BACKGROUND\n"); + for(unsigned int j = 0; j < cp.size(); j++){ + if(frame == cf[j]){continue;} + Eigen::Matrix4d p = poses[i].inverse() * cp[j]; + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, cf[j],mm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); + } + std::vector< std::vector > probs = getImageProbs(frames[i],9); + + std::vector frame_prior; + std::vector< std::vector > frame_connectionId; + std::vector< std::vector > frame_connectionStrength; + + frame_prior.resize(nr_pixels); + frame_connectionId.resize(nr_pixels); + frame_connectionStrength.resize(nr_pixels); + + + printf("starting partition\n"); + double start_part = getTime(); + unsigned int nr_data = width*height; + for(unsigned int j = 0; j < nr_data;j++){ + if(depthdata[j] == 0){ + frame_prior[j] = -1; + prior[offset+j] = -1; + continue; + } + + double p_fg = 0.499999999; + + if(occlusions[j] >= 1){ p_fg = maxprob;} + else if(overlaps[j] >= 1){ p_fg = 0.4;} + + p_fg = std::max(1-maxprob,std::min(maxprob,p_fg)); + + frame_prior[j] = p_fg; + prior[offset+j] = p_fg; + } + + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(w > 0 && w < width-1){ + int other = ind-1; + double p_same = std::min(probs[0][ind],maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + if(!std::isnan(weight) && weight > 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); + + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } + } + + if(h > 0 && h < height-1){ + int other = ind-width; + double p_same = std::min(probs[1][ind],maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + + if(!std::isnan(weight) && weight > 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); + + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } + } + +/* + if(w > 0 && w < width-1){ + double ax = 0.5; + double bx = 0.5; + for(unsigned int p = 0; p < probs.size(); p+=2){ + double pr = probs[p][ind]; + ax *= pr; + bx *= 1.0-pr; + } + double px = ax/(ax+bx); + + int other = ind-1; + //double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); + //double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); + //double p_same = std::min(probs[probs.size()-2][ind],maxprob_same); + //double p_same = std::max(1-maxprob_same,std::min(std::min(px,probs[probs.size()-2][ind]),maxprob_same)); + //double p_same = std::min(std::min(px,probs[probs.size()-2][ind]),maxprob_same); + //double p_same = std::min(px,maxprob_same); + +// double p_same = std::min(probs[probs.size()-2][ind],maxprob_same); +// if(detdata[ind] == 0){ +// p_same = std::min(px,maxprob_same); +// } + + float p_same = std::min(px,maxprob_same); + + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + if(!std::isnan(weight) && weight > 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); + + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } + } + + if(h > 0 && h < height-1){ + + double ay = 0.5; + double by = 0.5; + for(unsigned int p = 1; p < probs.size(); p+=2){ + double pr = probs[p][ind]; + ay *= pr; + by *= 1.0-pr; + } + double py = ay/(ay+by); + + int other = ind-width; + //double p_same = std::max(probs[probs.size()-1][ind],std::min(py,maxprob_same)); + + //double p_same = std::min(py,maxprob_same); + //double p_same = std::max(1-maxprob_same,std::min(std::min(py,probs[probs.size()-1][ind]),maxprob_same)); + //double p_same = std::min(probs[probs.size()-1][ind],maxprob_same); + + //double p_same = std::min(std::min(py,probs[probs.size()-1][ind]),maxprob_same); + double p_same = std::min(py,maxprob_same); + + //double p_same = std::min(probs[probs.size()-1][ind],maxprob_same); + //if(detdata[ind] == 0){ + // p_same = std::min(py,maxprob_same); + //} + + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + + if(!std::isnan(weight) && weight > 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); + + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } + } + */ + } + } + + + + double end_part = getTime(); + printf("part time: %10.10fs\n",end_part-start_part); + + //std::vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); + + Eigen::Matrix4d p = poses[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + pcl::PointXYZRGBNormal point; + point.x = m00*x + m01*y + m02*z + m03; + point.y = m10*x + m11*y + m12*z + m13; + point.z = m20*x + m21*y + m22*z + m23; + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; + + cloud->points.push_back(point); + } + } + } + + bool debugg2 = debugg; + if(debugg2){ + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + + point.r = 0; + point.g = 255; + point.b = 255; + + if(overlaps[ind] >= 1){ + point.r = 0; + point.g = 255; + point.b = 0; + } + if(occlusions[ind] >= 1){ + point.r = 255; + point.g = 0; + point.b = 0; + } + //cloud->points[ind] = point; + cloud2->points.push_back(point); + } + } + } + } + + for(unsigned int j = 0; j < nr_pixels; j++){ + occlusions[j] = 0; + } + + for(unsigned int j = 0; j < frames.size(); j++){ + if(frame == frames[j]){continue;} + Eigen::Matrix4d p = poses[i].inverse() * poses[j]; + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, frames[j],masks[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); + } -std::vector doInference(std::vector & prior, std::vector< std::vector > & connectionId, std::vector< std::vector > & connectionStrength){ - double start_inf = getTime(); - - unsigned int nr_data = prior.size(); - unsigned int nr_edges = 0; - for(unsigned int j = 0; j < nr_data;j++){ - nr_edges += connectionId[j].size(); - } - - gc::Graph * g = new gc::Graph(nr_data,nr_edges); - for(unsigned int i = 0; i < nr_data;i++){ - g -> add_node(); - double p_fg = prior[i]; - if(p_fg < 0){ - g -> add_tweights( i, 0, 0 ); - continue; - } - double p_bg = 1-p_fg; - double weightFG = -log(p_fg); - double weightBG = -log(p_bg); - g -> add_tweights( i, weightFG, weightBG ); - } - - for(unsigned int i = 0; i < nr_data;i++){ - for(unsigned int j = 0; j < connectionId[i].size();j++){ - double weight = 10*connectionStrength[i][j]; - g -> add_edge( i, connectionId[i][j], weight, weight ); - //try{g -> add_edge( i, connectionId[i][j], weight, weight );} - //catch(std::exception ex){printf("%i %i -> weight: %20.20f\n",i,connectionId[i],weight);exit(0);} - } - } - - g -> maxflow(); - - std::vector labels; - labels.resize(nr_data); - for(unsigned int i = 0; i < nr_data;i++){labels[i] = g->what_segment(i);} - delete g; - - double end_inf = getTime(); - printf("nr data: %i nr edges: %i inference time: %10.10fs\n",nr_data,nr_edges,end_inf-start_inf); - - return labels; -} + //Time to compute external masks... + delete[] overlaps; + delete[] occlusions; + } -void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, double debugg){ + //std::vector global_labels = doInference(prior,connectionId,connectionStrength); - unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); - float * src_normalsdata = (float *)(frame1->normals.data); + for(unsigned int i = 0; i < interframe_connectionId.size(); i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size(); j++){ + connectionId[i].push_back(interframe_connectionId[i][j]); + connectionStrength[i].push_back(interframe_connectionStrength[i][j]); + } + } + std::vector improvedglobal_labels = doInference(prior,connectionId,connectionStrength); + + for(unsigned int f = 0; f < frames.size(); f++){ + int offset = offsets[f]; + RGBDFrame * frame = frames[f]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + // unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + unsigned int nr_pixels = frame->camera->width * frame->camera->height; + + // cv::Mat originalmask; + // originalmask.create(height,width,CV_8UC1); + // unsigned char * originaldata = (unsigned char *)(originalmask.data); + + cv::Mat improvedmask; + improvedmask.create(height,width,CV_8UC1); + unsigned char * improveddata = (unsigned char *)(improvedmask.data); + + // cv::Mat diffmask; + // diffmask.create(height,width,CV_8UC3); + // unsigned char * diffdata = (unsigned char *)(diffmask.data); + + for(unsigned int i = 0; i < nr_pixels;i++){ + //internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); + //originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph::SOURCE)); + improveddata[i] = 255.0*((depthdata[i] == 0) || (improvedglobal_labels[i+offset] == gc::Graph::SOURCE)); + // if(improveddata[i] < originaldata[i]){ + // diffdata[3*i+0] = 0; + // diffdata[3*i+1] = 255; + // diffdata[3*i+2] = 0; + // }else if(improveddata[i] > originaldata[i]){ + // diffdata[3*i+0] = 0; + // diffdata[3*i+1] = 0; + // diffdata[3*i+2] = 255; + // }else{ + // diffdata[3*i+0] = 0; + // diffdata[3*i+1] = 0; + // diffdata[3*i+2] = 0; + // } - unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); - unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); - Camera * src_camera = frame1->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - - unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); - float * dst_normalsdata = (float *)(frame2->normals.data); - - Camera * dst_camera = frame2->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - //bool debugg = true; - pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); - if(debugg){ - - src_cloud->points.resize(src_width*src_height); - for(unsigned int src_w = 0; src_w < src_width;src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; - - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; - point.r = 255; - point.g = 0; - point.b = 0; - src_cloud->points[src_ind] = point; - } - } - } - - dst_cloud->points.resize(dst_width*dst_height); - for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - int dst_ind = dst_h*dst_width+dst_w; - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - pcl::PointXYZRGBNormal point; - point.x = dst_x; - point.y = dst_y; - point.z = dst_z; - point.r = 0; - point.g = 0; - point.b = 255; - dst_cloud->points[dst_ind] = point; - } - } - } - } - - std::vector inds; - std::vector distances; - - for(unsigned int src_w = 0; src_w < src_width;src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - - - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; + } +// frame->show(false); +// //cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); +// //cv::imshow( "Original", originalmask ); +// cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "Improved", improvedmask ); +// //cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); +// //cv::imshow( "Difference", diffmask ); +// cv::waitKey(0); + newmasks.push_back(improvedmask); + } - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - if(tz <= 0){continue;} - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; + if(debugg){ - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; + for(unsigned int i = 0; i < bgcf.size(); i++){ + RGBDFrame * frame = bgcf[i]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; - double dst_noise = dst_z * dst_z; - double point_noise = src_z * src_z; + Eigen::Matrix4d p = bgcp[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / sqrt(dst_noise*dst_noise + point_noise*point_noise); - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; - if(dst_detdata[dst_ind] != 0){continue;} - if(src_detdata[src_ind] != 0){continue;} + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; - if(fabs(d) < 0.02){//If close, according noises, and angle of the surfaces similar: FUSE - if(surface_angle > 0.9){ - overlaps[src_ind] = std::max(overlaps[src_ind],0.9); - // overlaps[src_ind]++; - // if(debugg){ - //// dst_cloud->points[dst_ind].r = 0; - //// dst_cloud->points[dst_ind].g = 255; - //// dst_cloud->points[dst_ind].b = 0; - // src_cloud->points[src_ind].r = 0; - // src_cloud->points[src_ind].g = 255; - // src_cloud->points[src_ind].b = 0; - // } - } - }else if(tz < dst_z){ - occlusions[src_ind] = std::max(occlusions[src_ind],0.9); - if(debugg){ - src_cloud->points[src_ind].r = 255; - src_cloud->points[src_ind].g = 0; - src_cloud->points[src_ind].b = 255; - } - } - } - } - } - } - } - - if(debugg){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); - viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); - viewer->spin(); - } -} + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; -void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ - int tot_nr_pixels = 0; - std::vector offsets; - - for(unsigned int i = 0; i < frames.size(); i++){ - offsets.push_back(tot_nr_pixels); - unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; - tot_nr_pixels += nr_pixels; - } - printf("tot_nr_pixels: %i\n",tot_nr_pixels); - - - int current_point = 0; - double * priors = new double[3*tot_nr_pixels]; - double * priors_weight = new double[3*tot_nr_pixels]; - double * estimates = new double[3*tot_nr_pixels]; - double * points = new double[3*tot_nr_pixels]; - double * noise = new double[3*tot_nr_pixels]; - double * colors = new double[3*tot_nr_pixels]; - double * normals = new double[3*tot_nr_pixels]; - - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - - for(unsigned int i = 0; i < frames.size(); i++){ - int offset = offsets[i]; - RGBDFrame * frame = frames[i]; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - float * normalsdata = (float *)(frame->normals.data); - - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - - - unsigned int nr_pixels = width * height; - double * current_overlaps = new double[nr_pixels]; - double * current_occlusions = new double[nr_pixels]; - for(unsigned int j = 0; j < nr_pixels; j++){ - current_overlaps[j] = 0; - current_occlusions[j] = 0; - } - - for(unsigned int j = 0; j < frames.size(); j++){ - if(i == j){continue;} - Eigen::Matrix4d p = poses[i].inverse() * poses[j]; - getDynamicWeights(p.inverse(),frames[i], current_overlaps, current_occlusions, frames[j],false); - } - - - double * bg_overlaps = new double[nr_pixels]; - double * bg_occlusions = new double[nr_pixels]; - for(unsigned int j = 0; j < nr_pixels; j++){ - bg_overlaps[j] = 0; - bg_occlusions[j] = 0; - } - - for(unsigned int j = 0; j < bgcf.size(); j++){ - Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; - getDynamicWeights(p.inverse(),frames[i], bg_overlaps, bg_occlusions, bgcf[j],false); - } - - Eigen::Matrix4d p = poses[i]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - - float minprob = 0.01; - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - float p_moving = current_occlusions[ind]; - float p_dynamic = std::max((bg_occlusions[ind]-p_moving)*current_overlaps[ind],0.0); - p_dynamic += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); - p_moving += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); - - float p_static = std::max(bg_overlaps[ind]-p_moving-p_dynamic,0.0); - - float leftover = 1-p_moving-p_dynamic-p_static; - float notMoving = current_overlaps[ind]; - - float bias = 0.0001; - float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; - float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; - float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; - - float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover) + minprob; - float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover) + minprob; - float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover) + 2.0*minprob; - - float norm = p_moving_tot+p_dynamic_tot+p_static_tot; - float tx = normalsdata[3*ind+0]; - float ty = normalsdata[3*ind+1]; - float tz = normalsdata[3*ind+2]; - p_moving_tot /= norm; - p_dynamic_tot/= norm; - p_static_tot /= norm; - - priors[3*current_point+0] = p_moving_tot; - priors[3*current_point+1] = p_dynamic_tot; - priors[3*current_point+2] = p_static_tot; - priors_weight[3*current_point+0] = -log(1-p_moving_tot); - priors_weight[3*current_point+1] = -log(1-p_dynamic_tot); - priors_weight[3*current_point+2] = -log(1-p_static_tot); - estimates[3*current_point+0] = priors[3*current_point+0]; - estimates[3*current_point+1] = priors[3*current_point+1]; - estimates[3*current_point+2] = priors[3*current_point+2]; - points[3*current_point+0] = m00*x + m01*y + m02*z + m03; - points[3*current_point+1] = m10*x + m11*y + m12*z + m13; - points[3*current_point+2] = m20*x + m21*y + m22*z + m23; - noise[current_point] = z*z; - colors[3*current_point+0] = rgbdata[3*ind+0]; - colors[3*current_point+1] = rgbdata[3*ind+1]; - colors[3*current_point+2] = rgbdata[3*ind+2]; - normals[3*current_point+0] = -2; - normals[3*current_point+1] = -2; - normals[3*current_point+2] = -2; - if(tx != 2){ - normals[3*current_point+0] = m00*tx + m01*ty + m02*tz; - normals[3*current_point+1] = m10*tx + m11*ty + m12*tz; - normals[3*current_point+2] = m20*tx + m21*ty + m22*tz; - } - current_point++; - - pcl::PointXYZRGBNormal point; - point.x = m00*x + m01*y + m02*z + m03; - point.y = m10*x + m11*y + m12*z + m13; - point.z = m20*x + m21*y + m22*z + m23; - point.b = p_moving_tot*255.0; - point.g = p_dynamic_tot*255.0; - point.r = p_static_tot*255.0; - - cloud->points.push_back(point); - } - } - } - - // viewer->removeAllPointClouds(); - // viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - // viewer->spin(); - - - // for(unsigned int w = 0; w < width;w++){ - // for(unsigned int h = 0; h < height;h++){ - // int ind = h*width+w; - // float z = idepth*float(depthdata[ind]); - // if(z > 0){ - // float x = (float(w) - cx) * z * ifx; - // float y = (float(h) - cy) * z * ify; - - // pcl::PointXYZRGBNormal point; - // point.x = m00*x + m01*y + m02*z + m03; - // point.y = m10*x + m11*y + m12*z + m13; - // point.z = m20*x + m21*y + m22*z + m23; - // point.b = rgbdata[3*ind+0]; - // point.g = rgbdata[3*ind+1]; - // point.r = rgbdata[3*ind+2]; - // cloud->points.push_back(point); - // } - // } - // } - - - delete[] current_occlusions; - delete[] current_overlaps; - delete[] bg_occlusions; - delete[] bg_overlaps; - } + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; - if(debugg){ - for(unsigned int i = 0; i < current_point; i++){ - cloud->points[i].r = estimates[3*i+0]*255.0; - cloud->points[i].g = estimates[3*i+1]*255.0; - cloud->points[i].b = estimates[3*i+2]*255.0; + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; + + bgcloud->points.push_back(point); + } + } + } } + /* + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud1, pcl::visualization::PointCloudColorHandlerRGBField(cloud1), "scloud"); + viewer->spin(); viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + viewer->spin(); + */ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "scloud"); + //viewer->addPointCloud (bgcloud, pcl::visualization::PointCloudColorHandlerRGBField(bgcloud), "bgcloud"); viewer->spin(); - } + viewer->removeAllPointClouds(); - ArrayData3D * a3d = new ArrayData3D; - a3d->data = points; - a3d->rows = current_point; - Tree3d * tree = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); - tree->buildIndex(); - - const int find_nr_neighbours = 10; - - std::vector > neighbour_inds; - std::vector > neighbour_weights; - neighbour_inds.resize(current_point); - neighbour_weights.resize(current_point); - std::set neighbours_list; - - for(unsigned int i = 0; i < current_point; i++){ - if(i%100000 == 0){printf("%i/%i -> %f\n",i,current_point,float(i)/float(current_point));} - double qp [3]; - qp[0] = points[3*i+0]; - qp[1] = points[3*i+1]; - qp[2] = points[3*i+2]; - size_t ret_index [find_nr_neighbours]; - double out_dist_sqr [find_nr_neighbours]; - nanoflann::KNNResultSet resultSet(find_nr_neighbours); - resultSet.init(ret_index, out_dist_sqr ); - tree->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); - - for(unsigned int j = 1; j < find_nr_neighbours; j++){ - int ind = ret_index[j]; - float weight = 1.00;//Make based on distance, normal and color(if same frame) - char buff [1024]; - sprintf(buff,"%i_%i",i,ind); - if(neighbours_list.count(buff) == 0){ - neighbours_list.insert(buff); - - neighbour_inds[ind].push_back(i); - neighbour_weights[ind].push_back(weight); - - neighbour_inds[i].push_back(ind); - neighbour_weights[i].push_back(weight); - } - } - } - - for(int it = 0; it < 1000; it++){ - if(debugg){ - for(unsigned int i = 0; i < current_point; i++){ - cloud->points[i].r = estimates[3*i+0]*255.0; - cloud->points[i].g = estimates[3*i+1]*255.0; - cloud->points[i].b = estimates[3*i+2]*255.0; - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - } - - double total_change = 0; - double max_change = 0; - for(unsigned int i = 0; i < current_point; i++){ - if(i%100000 == 0){printf("%i/%i -> %f\n",i,current_point,float(i)/float(current_point));} - float score0 = priors_weight[3*i+0]; - float score1 = priors_weight[3*i+1]; - float score2 = priors_weight[3*i+2]; - std::vector & neighbours = neighbour_inds[i]; - std::vector & weights = neighbour_weights[i]; - unsigned int nr_neighbours = neighbours.size(); - - - - for(unsigned int j = 0; j < nr_neighbours; j++){ - int ind = neighbours[j]; - float weight = weights[j]; - float e0 = estimates[3*ind+0]; - float e1 = estimates[3*ind+1]; - float e2 = estimates[3*ind+2]; - float minval = 0;//std::min(std::min(e0,e1),e2); - score0+=weight*(e0-minval); - score1+=weight*(e1-minval); - score2+=weight*(e2-minval); - } - - float p0 = 1-exp(-score0); - float p1 = 1-exp(-score1); - float p2 = 1-exp(-score2); - float norm = p0+p1+p2; - p0 /= norm; - p1 /= norm; - p2 /= norm; - float e0 = estimates[3*i+0]; - float e1 = estimates[3*i+1]; - float e2 = estimates[3*i+2]; - double change = fabs(e0-p0)+fabs(e1-p1)+fabs(e2-p2); - - if(true && change > max_change){ - printf("%i/%i -> ",i,current_point); - printf("prior: %f %f %f -> \n",priors_weight[3*i+0],priors_weight[3*i+1],priors_weight[3*i+2]); - - for(unsigned int j = 0; j < nr_neighbours; j++){ - int ind = neighbours[j]; - float weight = weights[j]; - float e0 = estimates[3*ind+0]; - float e1 = estimates[3*ind+1]; - float e2 = estimates[3*ind+2]; - float minval = std::min(std::min(e0,e1),e2); - printf("%f ,",weight*(e0-minval)); - printf("%f ,",weight*(e1-minval)); - printf("%f\n",weight*(e2-minval)); - } - - - printf("total: %f %f %f\n",score0,score1,score2); - - printf("\n"); - } - - total_change += change; - max_change = std::max(change,max_change); - - estimates[3*i+0] = p0; - estimates[3*i+1] = p1; - estimates[3*i+2] = p2; - - - cloud->points[i].r = std::min((change)*255.0,255.0); - cloud->points[i].g = std::min((1-change)*255.0,255.0); - cloud->points[i].b = 0; - } - printf("mean_change: %f max change: %f\n",total_change/float(current_point),max_change); - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - - } - - - - - delete[] estimates; - delete[] priors_weight; - delete[] priors; - delete[] points; - delete[] colors; - delete[] normals; - - exit(0); + } +} + return newmasks; } +std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector oldmasks){ + std::vector newmasks; + for(unsigned int i = 0; i < cf.size(); i++){ + unsigned short * depthdata = (unsigned short *)(cf[i]->depth.data); -vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg){ -// -std::vector newmasks; -//for(double test = 0.99; test > 0.00000001; test *= 0.1) -{ -// double maxprob_same = 1-test;//0.99; + Camera * camera = cf[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; - double maxprob_same = 0.999999; - double maxprob = 0.7; + unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; + double * overlaps = new double[nr_pixels]; + double * occlusions = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + overlaps[j] = 0; + occlusions[j] = 0; + } - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud1 (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); - pcl::PointCloud::Ptr bgcloud (new pcl::PointCloud); + std::vector< std::vector > interframe_connectionId_tmp; + std::vector< std::vector > interframe_connectionStrength_tmp; - int tot_nr_pixels = 0; - std::vector offsets; + // for(unsigned int j = 0; j < bgcp.size(); j++){ + // if(frame == bgcf[j]){continue;} + // Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; + // getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); + // } - for(unsigned int i = 0; i < frames.size(); i++){ - offsets.push_back(tot_nr_pixels); - unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; - tot_nr_pixels += nr_pixels; - } + for(unsigned int j = 0; j < cf.size(); j++){ + if(i == j){continue;} + Eigen::Matrix4d p = cp[i].inverse() * cp[j]; + getDynamicWeights(false,p.inverse(), cf[i], overlaps, occlusions, cf[j],oldmasks[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp); + } + std::vector< std::vector > probs = getImageProbs(cf[i],5); + + printf("starting partition\n"); + double start_part = getTime(); + unsigned int nr_data = width*height; + gc::Graph *g = new gc::Graph( nr_data, width*(height-1) + (width-1)*height); + + double maxprob = 0.7; + for(unsigned int j = 0; j < nr_data;j++){ + g -> add_node(); + if(depthdata[j] == 0){ + g -> add_tweights(j,0,0); + continue; + } - //Graph... - std::vector prior; - std::vector< std::vector > connectionId; - std::vector< std::vector > connectionStrength; - std::vector< std::vector > interframe_connectionId; - std::vector< std::vector > interframe_connectionStrength; + double p_fg = 0.499; - prior.resize(tot_nr_pixels); - connectionId.resize(tot_nr_pixels); - connectionStrength.resize(tot_nr_pixels); - interframe_connectionId.resize(tot_nr_pixels); - interframe_connectionStrength.resize(tot_nr_pixels); + if(occlusions[j] >= 1){ p_fg = maxprob;} + else if(overlaps[j] >= 1){ p_fg = 0.4;} + p_fg = std::max(1-maxprob,std::min(maxprob,p_fg)); + double p_bg = 1-p_fg; + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + g -> add_tweights( j, weightFG, weightBG ); + } - for(unsigned int i = 0; i < frames.size(); i++){ - printf("frame: %i\n",i); - int offset = offsets[i]; - RGBDFrame * frame = frames[i]; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned char * detdata = (unsigned char *)(frame->det_dilate.data); - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; + double maxprob_same = 0.999999999; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + double ax = 0.5; + double bx = 0.5; + for(unsigned int p = 0; p < probs.size(); p+=2){ + double pr = probs[p][ind]; + ax *= pr; + bx *= 1.0-pr; + } + double px = ax/(ax+bx); - unsigned int nr_pixels = frame->camera->width * frame->camera->height; - double * overlaps = new double[nr_pixels]; - double * occlusions = new double[nr_pixels]; - for(unsigned int j = 0; j < nr_pixels; j++){ - overlaps[j] = 0; - occlusions[j] = 0; - } + double ay = 0.5; + double by = 0.5; + for(unsigned int p = 1; p < probs.size(); p+=2){ + double pr = probs[p][ind]; + ay *= pr; + by *= 1.0-pr; + } + double py = ay/(ay+by); - std::vector< std::vector > interframe_connectionId_tmp; - std::vector< std::vector > interframe_connectionStrength_tmp; + if(w > 0){ + int other = ind-1; + double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + g -> add_edge( ind, other, weight, weight ); + } - for(unsigned int j = 0; j < bgcp.size(); j++){ - if(frame == bgcf[j]){continue;} - Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); - } - - for(unsigned int j = 0; j < nr_pixels; j++){ - occlusions[j] = 0; - } - printf("CHECK AGAINST BACKGROUND\n"); - for(unsigned int j = 0; j < cp.size(); j++){ - if(frame == cf[j]){continue;} - Eigen::Matrix4d p = poses[i].inverse() * cp[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, cf[j],mm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); - } - std::vector< std::vector > probs = getImageProbs(frames[i],9); + if(h > 0){ + int other = ind-width; + double p_same = std::max(probs[probs.size()-1][ind],std::min(py,maxprob_same)); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + g -> add_edge( ind, other, weight, weight ); + } + } + } - std::vector frame_prior; - std::vector< std::vector > frame_connectionId; - std::vector< std::vector > frame_connectionStrength; + int flow = g -> maxflow(); + printf("flow: %i\n",flow); - frame_prior.resize(nr_pixels); - frame_connectionId.resize(nr_pixels); - frame_connectionStrength.resize(nr_pixels); + double end_part = getTime(); + printf("part time: %10.10fs\n",end_part-start_part); - printf("starting partition\n"); - double start_part = getTime(); - unsigned int nr_data = width*height; - for(unsigned int j = 0; j < nr_data;j++){ - if(depthdata[j] == 0){ - frame_prior[j] = -1; - prior[offset+j] = -1; - continue; - } + cv::Mat internalmask; - double p_fg = 0.499999999; + internalmask.create(height,width,CV_8UC1); + unsigned char * internaldata = (unsigned char *)(internalmask.data); + for(unsigned int i = 0; i < width*height;i++){internaldata[i] = 255.0*(g->what_segment(i) == gc::Graph::SOURCE);} - if(occlusions[j] >= 1){ p_fg = maxprob;} - else if(overlaps[j] >= 1){ p_fg = 0.4;} + cv::imshow( "rgb", cf[i]->rgb ); + cv::imshow( "internalmask", internalmask ); + cv::waitKey(30); - p_fg = std::max(1-maxprob,std::min(maxprob,p_fg)); + newmasks.push_back(internalmask); + //Time to compute external masks... + delete[] overlaps; + delete[] occlusions; + } - frame_prior[j] = p_fg; - prior[offset+j] = p_fg; - } + return newmasks; +} +OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * src_modelmask, RGBDFrame * dst, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step, bool debugg){ + OcclusionScore oc; + + unsigned char * src_maskdata = (unsigned char *)(src_modelmask->mask.data); + unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(src->depth.data); + float * src_normalsdata = (float *)(src->normals.data); + + unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); + float * dst_normalsdata = (float *)(dst->normals.data); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + Camera * src_camera = src->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + Camera * dst_camera = dst->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + //bool debugg = false; + cv::Mat debugg_img; + unsigned char * debugg_img_data; + if(debugg){ + debugg_img = src->rgb.clone(); + debugg_img_data = (unsigned char *)(debugg_img.data); + } - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - if(w > 0 && w < width-1){ - int other = ind-1; - double p_same = std::min(probs[0][ind],maxprob_same); - double not_p_same = 1-p_same; - double weight = -log(not_p_same); - if(!std::isnan(weight) && weight > 0){ - frame_connectionId[ind].push_back(other); - frame_connectionStrength[ind].push_back(weight); + std::vector residuals; + std::vector angleweights; + std::vector weights; + std::vector ws; + std::vector hs; - connectionId[ind+offset].push_back(other+offset); - connectionStrength[ind+offset].push_back(weight); + std::vector dst_ws; + std::vector dst_hs; + + residuals.reserve(src_width*src_height); + angleweights.reserve(src_width*src_height); + weights.reserve(src_width*src_height); + if(debugg){ + ws.reserve(src_width*src_height); + hs.reserve(src_width*src_height); + dst_ws.reserve(dst_width*dst_height); + dst_hs.reserve(dst_width*dst_height); + } + + double sum = 0; + double count = 0; + + std::vector & testw = src_modelmask->testw; + std::vector & testh = src_modelmask->testh; + + unsigned int test_nrdata = testw.size(); + unsigned int indstep = step; + // int indstep = std::max(1.0,double(test_nrdata)/double(step)); + // printf("indstep: %i\n",indstep); + // for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){ + // //for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ + // // for(unsigned int src_h = 0; src_h < src_height;src_h++){ + // unsigned int src_w = testw[ind]; + // unsigned int src_h = testh[ind]; + + // int src_ind0 = src_h*src_width+src_w-1; + // int src_ind1 = src_h*src_width+src_w; + // int src_ind2 = src_h*src_width+src_w+1; + // if(src_maskdata[src_ind0] == 255 && src_maskdata[src_ind1] == 255 && src_maskdata[src_ind2] == 255){// && p.z > 0 && !isnan(p.normal_x)){ + // float z0 = src_idepth*float(src_depthdata[src_ind0]); + // float z1 = src_idepth*float(src_depthdata[src_ind1]); + // float z2 = src_idepth*float(src_depthdata[src_ind2]); + + // double diff0 = (z0-z1)/(z0*z0+z1*z1); + // double diff1 = (z2-z1)/(z2*z2+z1*z1); + // if( fabs(diff0) < fabs(diff1)){ + // if(diff0 != 0){ + // sum += diff0*diff0; + // count++; + // } + // }else{ + // if(diff1 != 0){ + // sum += diff1*diff1; + // count++; + // } + // } + // } + // } + // double me = sqrt(sum/(count+1)); + // double pred = 1.3*sqrt(2)*me; + /* + for(unsigned int ind = 0; ind < test_nrdata;ind++){ + //for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ + // for(unsigned int src_h = 0; src_h < src_height;src_h++){ + unsigned int src_w = testw[ind]; + unsigned int src_h = testh[ind]; + + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ + float z = src_idepth*float(src_depthdata[src_ind]); + float nx = src_normalsdata[3*src_ind+0]; + if(z > 0 && nx != 2){ + //if(z > 0){ + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + //float tnx = m00*nx + m01*ny + m02*nz; + //float tny = m10*nx + m11*ny + m12*nz; + float tnz = m20*nx + m21*ny + m22*nz; + + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + if(dst_z > 0){ + float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 + residuals.push_back(diff_z); + if(debugg){ + ws.push_back(src_w); + hs.push_back(src_h); + } } } + } + } + } +*/ + for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){ + unsigned int src_w = testw[ind]; + unsigned int src_h = testh[ind]; + + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] == 255){ + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + //if(src_z > 0){ + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; - if(h > 0 && h < height-1){ - int other = ind-width; - double p_same = std::min(probs[1][ind],maxprob_same); - double not_p_same = 1-p_same; - double weight = -log(not_p_same); + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; - if(!std::isnan(weight) && weight > 0){ - frame_connectionId[ind].push_back(other); - frame_connectionStrength[ind].push_back(weight); + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - connectionId[ind+offset].push_back(other+offset); - connectionStrength[ind+offset].push_back(weight); - } - } + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; -/* - if(w > 0 && w < width-1){ - double ax = 0.5; - double bx = 0.5; - for(unsigned int p = 0; p < probs.size(); p+=2){ - double pr = probs[p][ind]; - ax *= pr; - bx *= 1.0-pr; - } - double px = ax/(ax+bx); - - int other = ind-1; - //double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); - //double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); - //double p_same = std::min(probs[probs.size()-2][ind],maxprob_same); - //double p_same = std::max(1-maxprob_same,std::min(std::min(px,probs[probs.size()-2][ind]),maxprob_same)); - //double p_same = std::min(std::min(px,probs[probs.size()-2][ind]),maxprob_same); - //double p_same = std::min(px,maxprob_same); + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; -// double p_same = std::min(probs[probs.size()-2][ind],maxprob_same); -// if(detdata[ind] == 0){ -// p_same = std::min(px,maxprob_same); -// } + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - float p_same = std::min(px,maxprob_same); + double info = (src_z*src_z+dst_z*dst_z); - double not_p_same = 1-p_same; - double weight = -log(not_p_same); - if(!std::isnan(weight) && weight > 0){ - frame_connectionId[ind].push_back(other); - frame_connectionStrength[ind].push_back(weight); + //double d = mysign(dst_z-tz)*fabs(dst_nx*(dst_x-tx) + dst_ny*(dst_y-ty) + dst_nz*(dst_z-tz)); + double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + residuals.push_back(d/info); - connectionId[ind+offset].push_back(other+offset); - connectionStrength[ind+offset].push_back(weight); - } - } + //float diff_z = (dst_z-tz)/(src_z*src_z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 + //residuals.push_back(diff_z); - if(h > 0 && h < height-1){ - double ay = 0.5; - double by = 0.5; - for(unsigned int p = 1; p < probs.size(); p+=2){ - double pr = probs[p][ind]; - ay *= pr; - by *= 1.0-pr; - } - double py = ay/(ay+by); - int other = ind-width; - //double p_same = std::max(probs[probs.size()-1][ind],std::min(py,maxprob_same)); - //double p_same = std::min(py,maxprob_same); - //double p_same = std::max(1-maxprob_same,std::min(std::min(py,probs[probs.size()-1][ind]),maxprob_same)); - //double p_same = std::min(probs[probs.size()-1][ind],maxprob_same); - //double p_same = std::min(std::min(py,probs[probs.size()-1][ind]),maxprob_same); - double p_same = std::min(py,maxprob_same); + // float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + // float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + //float angle = (tnx*dst_x+tny*dst_y+tnz*dst_z)/sqrt(dst_x*dst_x + dst_y*dst_y + dst_z*dst_z); + //weights.push_back(1-angle); + //weights.push_back(ny*dst_ny); - //double p_same = std::min(probs[probs.size()-1][ind],maxprob_same); - //if(detdata[ind] == 0){ - // p_same = std::min(py,maxprob_same); - //} - double not_p_same = 1-p_same; - double weight = -log(not_p_same); + double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); + double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); - if(!std::isnan(weight) && weight > 0){ - frame_connectionId[ind].push_back(other); - frame_connectionStrength[ind].push_back(weight); + double dist_src = sqrt(src_x*src_x+src_y*src_y+src_z*src_z); + double angle_src = fabs((src_x*src_nx+src_y*src_ny+src_z*src_nz)/dist_src); + weights.push_back(angle_src * angle_dst); - connectionId[ind+offset].push_back(other+offset); - connectionStrength[ind+offset].push_back(weight); - } - } - */ - } - } - - - - double end_part = getTime(); - printf("part time: %10.10fs\n",end_part-start_part); - - //std::vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); - - Eigen::Matrix4d p = poses[i]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - pcl::PointXYZRGBNormal point; - point.x = m00*x + m01*y + m02*z + m03; - point.y = m10*x + m11*y + m12*z + m13; - point.z = m20*x + m21*y + m22*z + m23; - point.b = rgbdata[3*ind+0]; - point.g = rgbdata[3*ind+1]; - point.r = rgbdata[3*ind+2]; - - cloud->points.push_back(point); - } - } - } - - bool debugg2 = debugg; - if(debugg2){ - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; - - point.r = 0; - point.g = 255; - point.b = 255; - - if(overlaps[ind] >= 1){ - point.r = 0; - point.g = 255; - point.b = 0; - } - if(occlusions[ind] >= 1){ - point.r = 255; - point.g = 0; - point.b = 0; - } - //cloud->points[ind] = point; - cloud2->points.push_back(point); - } - } - } - } - - for(unsigned int j = 0; j < nr_pixels; j++){ - occlusions[j] = 0; - } - - for(unsigned int j = 0; j < frames.size(); j++){ - if(frame == frames[j]){continue;} - Eigen::Matrix4d p = poses[i].inverse() * poses[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, frames[j],masks[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); - } - - //Time to compute external masks... - delete[] overlaps; - delete[] occlusions; - } - - //std::vector global_labels = doInference(prior,connectionId,connectionStrength); - - for(unsigned int i = 0; i < interframe_connectionId.size(); i++){ - for(unsigned int j = 0; j < interframe_connectionId[i].size(); j++){ - connectionId[i].push_back(interframe_connectionId[i][j]); - connectionStrength[i].push_back(interframe_connectionStrength[i][j]); - } - } - - std::vector improvedglobal_labels = doInference(prior,connectionId,connectionStrength); - - for(unsigned int f = 0; f < frames.size(); f++){ - int offset = offsets[f]; - RGBDFrame * frame = frames[f]; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - // unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - unsigned int nr_pixels = frame->camera->width * frame->camera->height; - - // cv::Mat originalmask; - // originalmask.create(height,width,CV_8UC1); - // unsigned char * originaldata = (unsigned char *)(originalmask.data); - - cv::Mat improvedmask; - improvedmask.create(height,width,CV_8UC1); - unsigned char * improveddata = (unsigned char *)(improvedmask.data); - - // cv::Mat diffmask; - // diffmask.create(height,width,CV_8UC3); - // unsigned char * diffdata = (unsigned char *)(diffmask.data); - - for(unsigned int i = 0; i < nr_pixels;i++){ - //internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); - //originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph::SOURCE)); - improveddata[i] = 255.0*((depthdata[i] == 0) || (improvedglobal_labels[i+offset] == gc::Graph::SOURCE)); - // if(improveddata[i] < originaldata[i]){ - // diffdata[3*i+0] = 0; - // diffdata[3*i+1] = 255; - // diffdata[3*i+2] = 0; - // }else if(improveddata[i] > originaldata[i]){ - // diffdata[3*i+0] = 0; - // diffdata[3*i+1] = 0; - // diffdata[3*i+2] = 255; - // }else{ - // diffdata[3*i+0] = 0; - // diffdata[3*i+1] = 0; - // diffdata[3*i+2] = 0; - // } - - - } + if(debugg){ + ws.push_back(src_w); + hs.push_back(src_h); -// frame->show(false); -// //cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); -// //cv::imshow( "Original", originalmask ); -// cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "Improved", improvedmask ); -// //cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); -// //cv::imshow( "Difference", diffmask ); -// cv::waitKey(0); - newmasks.push_back(improvedmask); - } - - - if(debugg){ - - - for(unsigned int i = 0; i < bgcf.size(); i++){ - RGBDFrame * frame = bgcf[i]; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - Eigen::Matrix4d p = bgcp[i]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; - - point.b = rgbdata[3*ind+0]; - point.g = rgbdata[3*ind+1]; - point.r = rgbdata[3*ind+2]; - - bgcloud->points.push_back(point); - } - } - } - } - /* - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud1, pcl::visualization::PointCloudColorHandlerRGBField(cloud1), "scloud"); - viewer->spin(); - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - viewer->spin(); - */ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "scloud"); - //viewer->addPointCloud (bgcloud, pcl::visualization::PointCloudColorHandlerRGBField(bgcloud), "bgcloud"); - viewer->spin(); + dst_ws.push_back(dst_w); + dst_hs.push_back(dst_h); + } + } + } + } + } + } - viewer->removeAllPointClouds(); - } -} - return newmasks; -} -std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector oldmasks){ - std::vector newmasks; - for(unsigned int i = 0; i < cf.size(); i++){ - unsigned short * depthdata = (unsigned short *)(cf[i]->depth.data); - - Camera * camera = cf[i]->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - - unsigned int nr_pixels = cf[i]->camera->width * cf[i]->camera->height; - double * overlaps = new double[nr_pixels]; - double * occlusions = new double[nr_pixels]; - for(unsigned int j = 0; j < nr_pixels; j++){ - overlaps[j] = 0; - occlusions[j] = 0; - } - - - std::vector< std::vector > interframe_connectionId_tmp; - std::vector< std::vector > interframe_connectionStrength_tmp; - - // for(unsigned int j = 0; j < bgcp.size(); j++){ - // if(frame == bgcf[j]){continue;} - // Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; - // getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); - // } - - for(unsigned int j = 0; j < cf.size(); j++){ - if(i == j){continue;} - Eigen::Matrix4d p = cp[i].inverse() * cp[j]; - getDynamicWeights(false,p.inverse(), cf[i], overlaps, occlusions, cf[j],oldmasks[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp); - } - std::vector< std::vector > probs = getImageProbs(cf[i],5); - - printf("starting partition\n"); - double start_part = getTime(); - unsigned int nr_data = width*height; - gc::Graph *g = new gc::Graph( nr_data, width*(height-1) + (width-1)*height); - - double maxprob = 0.7; - for(unsigned int j = 0; j < nr_data;j++){ - g -> add_node(); - if(depthdata[j] == 0){ - g -> add_tweights(j,0,0); - continue; - } - - double p_fg = 0.499; - - if(occlusions[j] >= 1){ p_fg = maxprob;} - else if(overlaps[j] >= 1){ p_fg = 0.4;} - - p_fg = std::max(1-maxprob,std::min(maxprob,p_fg)); - - double p_bg = 1-p_fg; - double weightFG = -log(p_fg); - double weightBG = -log(p_bg); - g -> add_tweights( j, weightFG, weightBG ); - } - - double maxprob_same = 0.999999999; - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - - double ax = 0.5; - double bx = 0.5; - for(unsigned int p = 0; p < probs.size(); p+=2){ - double pr = probs[p][ind]; - ax *= pr; - bx *= 1.0-pr; - } - double px = ax/(ax+bx); - - double ay = 0.5; - double by = 0.5; - for(unsigned int p = 1; p < probs.size(); p+=2){ - double pr = probs[p][ind]; - ay *= pr; - by *= 1.0-pr; - } - double py = ay/(ay+by); - - if(w > 0){ - int other = ind-1; - double p_same = std::max(probs[probs.size()-2][ind],std::min(px,maxprob_same)); - double not_p_same = 1-p_same; - double weight = -log(not_p_same); - g -> add_edge( ind, other, weight, weight ); - } - - if(h > 0){ - int other = ind-width; - double p_same = std::max(probs[probs.size()-1][ind],std::min(py,maxprob_same)); - double not_p_same = 1-p_same; - double weight = -log(not_p_same); - g -> add_edge( ind, other, weight, weight ); - } - } - } - - int flow = g -> maxflow(); - printf("flow: %i\n",flow); - - - double end_part = getTime(); - printf("part time: %10.10fs\n",end_part-start_part); - - cv::Mat internalmask; - - internalmask.create(height,width,CV_8UC1); - unsigned char * internaldata = (unsigned char *)(internalmask.data); - for(unsigned int i = 0; i < width*height;i++){internaldata[i] = 255.0*(g->what_segment(i) == gc::Graph::SOURCE);} - - cv::imshow( "rgb", cf[i]->rgb ); - cv::imshow( "internalmask", internalmask ); - cv::waitKey(30); - - newmasks.push_back(internalmask); - //Time to compute external masks... - delete[] overlaps; - delete[] occlusions; - } - - return newmasks; -} -OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * src_modelmask, RGBDFrame * dst, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step, bool debugg){ - OcclusionScore oc; - - unsigned char * src_maskdata = (unsigned char *)(src_modelmask->mask.data); - unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(src->depth.data); - float * src_normalsdata = (float *)(src->normals.data); - - unsigned char * dst_maskdata = (unsigned char *)(dst_modelmask->mask.data); - unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); - float * dst_normalsdata = (float *)(dst->normals.data); - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - Camera * src_camera = src->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - - Camera * dst_camera = dst->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - - //bool debugg = false; - cv::Mat debugg_img; - unsigned char * debugg_img_data; - if(debugg){ - debugg_img = src->rgb.clone(); - debugg_img_data = (unsigned char *)(debugg_img.data); - } - - std::vector residuals; - std::vector angleweights; - std::vector weights; - std::vector ws; - std::vector hs; - - std::vector dst_ws; - std::vector dst_hs; - - residuals.reserve(src_width*src_height); - angleweights.reserve(src_width*src_height); - weights.reserve(src_width*src_height); - if(debugg){ - ws.reserve(src_width*src_height); - hs.reserve(src_width*src_height); - dst_ws.reserve(dst_width*dst_height); - dst_hs.reserve(dst_width*dst_height); - } - - double sum = 0; - double count = 0; - - std::vector & testw = src_modelmask->testw; - std::vector & testh = src_modelmask->testh; - - unsigned int test_nrdata = testw.size(); - unsigned int indstep = step; - // int indstep = std::max(1.0,double(test_nrdata)/double(step)); - // printf("indstep: %i\n",indstep); - // for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){ - // //for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ - // // for(unsigned int src_h = 0; src_h < src_height;src_h++){ - // unsigned int src_w = testw[ind]; - // unsigned int src_h = testh[ind]; - - // int src_ind0 = src_h*src_width+src_w-1; - // int src_ind1 = src_h*src_width+src_w; - // int src_ind2 = src_h*src_width+src_w+1; - // if(src_maskdata[src_ind0] == 255 && src_maskdata[src_ind1] == 255 && src_maskdata[src_ind2] == 255){// && p.z > 0 && !isnan(p.normal_x)){ - // float z0 = src_idepth*float(src_depthdata[src_ind0]); - // float z1 = src_idepth*float(src_depthdata[src_ind1]); - // float z2 = src_idepth*float(src_depthdata[src_ind2]); - - // double diff0 = (z0-z1)/(z0*z0+z1*z1); - // double diff1 = (z2-z1)/(z2*z2+z1*z1); - // if( fabs(diff0) < fabs(diff1)){ - // if(diff0 != 0){ - // sum += diff0*diff0; - // count++; - // } - // }else{ - // if(diff1 != 0){ - // sum += diff1*diff1; - // count++; - // } - // } - // } - // } - // double me = sqrt(sum/(count+1)); - // double pred = 1.3*sqrt(2)*me; - /* - for(unsigned int ind = 0; ind < test_nrdata;ind++){ - //for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ - // for(unsigned int src_h = 0; src_h < src_height;src_h++){ - unsigned int src_w = testw[ind]; - unsigned int src_h = testh[ind]; - - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ - float z = src_idepth*float(src_depthdata[src_ind]); - float nx = src_normalsdata[3*src_ind+0]; - if(z > 0 && nx != 2){ - //if(z > 0){ - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; - - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - - //float tnx = m00*nx + m01*ny + m02*nz; - //float tny = m10*nx + m11*ny + m12*nz; - float tnz = m20*nx + m21*ny + m22*nz; - - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - if(dst_z > 0){ - float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 - residuals.push_back(diff_z); - if(debugg){ - ws.push_back(src_w); - hs.push_back(src_h); - } - } - } - } - } - } -*/ - for(unsigned int ind = 0; ind < test_nrdata;ind+=indstep){ - unsigned int src_w = testw[ind]; - unsigned int src_h = testh[ind]; + // DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + // func->maxp = 1.0; + // func->update_size = true; + // func->zeromean = true; + // func->startreg = 0.0001; + // func->debugg_print = debugg; + // func->bidir = true; + // func->maxnoise = pred; + // func->reset(); + + DistanceWeightFunction2 * func = new DistanceWeightFunction2(); + func->f = THRESHOLD; + func->p = 0.01; + + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); + for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} + func->computeModel(X); + + Eigen::VectorXd W = func->getProbs(X); + + delete func; + + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + oc.score += weight*weights.at(i); + oc.occlusions += ocl*weights.at(i); + } + + //debugg = true; + if(debugg){ + pcl::PointCloud::Ptr scloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + scloud->points.resize(src_width*src_height); + dcloud->points.resize(dst_width*dst_height); + /* + for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] != 255){continue;} + float z = src_idepth*float(src_depthdata[src_ind]); + if(z > 0){ + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + scloud->points[src_ind].x = tx; + scloud->points[src_ind].y = ty; + scloud->points[src_ind].z = tz+2; + scloud->points[src_ind].r = 0; + scloud->points[src_ind].g = 0; + scloud->points[src_ind].b = 255; + } + } + } + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + unsigned int dst_ind = dst_h*dst_width+dst_w; + if(true || dst_maskdata[dst_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ + float z = dst_idepth*float(dst_depthdata[dst_ind]); + if(z > 0){// && (dst_w%3 == 0) && (dst_h%3 == 0)){ + float x = (float(dst_w) - dst_cx) * z * dst_ifx; + float y = (float(dst_h) - dst_cy) * z * dst_ify; + dcloud->points[dst_ind].x = x; + dcloud->points[dst_ind].y = y; + dcloud->points[dst_ind].z = z+2; + dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; + dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; + dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; + if(dst_maskdata[dst_ind] == 255){ + dcloud->points[dst_ind].r = 255; + dcloud->points[dst_ind].g = 000; + dcloud->points[dst_ind].b = 255; + } + } + } + } + } + viewer->removeAllPointClouds(); + //printf("%i showing results\n",__LINE__); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + */ + /* + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; + dcloud->points[dst_ind].x = 0; + dcloud->points[dst_ind].y = 0; + dcloud->points[dst_ind].z = 0; + } + } + + + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + int w = ws[i]; + int h = hs[i]; + unsigned int src_ind = h * src_width + w; + if(ocl > 0.01 || weight > 0.01){ + scloud->points[src_ind].r = 255.0*ocl; + scloud->points[src_ind].g = 0; + scloud->points[src_ind].b = 0; + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + //printf("%i showing results\n",__LINE__); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + + for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] != 255){continue;} + float z = src_idepth*float(src_depthdata[src_ind]); + if(z > 0){ + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + scloud->points[src_ind].x = tx; + scloud->points[src_ind].y = ty; + scloud->points[src_ind].z = tz+2; + scloud->points[src_ind].r = 0; + scloud->points[src_ind].g = 0; + scloud->points[src_ind].b = 255; + } + } + } + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; + if(true || dst_maskdata[dst_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ + float z = dst_idepth*float(dst_depthdata[dst_ind]); + if(z > 0){// && (dst_w%3 == 0) && (dst_h%3 == 0)){ + float x = (float(dst_w) - dst_cx) * z * dst_ifx; + float y = (float(dst_h) - dst_cy) * z * dst_ify; + dcloud->points[dst_ind].x = x; + dcloud->points[dst_ind].y = y; + dcloud->points[dst_ind].z = z+2; + dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; + dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; + dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; + if(dst_maskdata[dst_ind] == 255){ + dcloud->points[dst_ind].r = 255; + dcloud->points[dst_ind].g = 000; + dcloud->points[dst_ind].b = 255; + } + } + } + } + } + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + int w = ws[i]; + int h = hs[i]; + unsigned int src_ind = h * src_width + w; + if(ocl > 0.01 || weight > 0.01){ + scloud->points[src_ind].r = 255.0*ocl;//*weights.at(i); + scloud->points[src_ind].g = 255.0*weight;//*weights.at(i); + scloud->points[src_ind].b = 0; + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] == 255){ - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - //if(src_z > 0){ - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; + viewer->removeAllPointClouds(); - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - - double info = (src_z*src_z+dst_z*dst_z); - - //double d = mysign(dst_z-tz)*fabs(dst_nx*(dst_x-tx) + dst_ny*(dst_y-ty) + dst_nz*(dst_z-tz)); - double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - residuals.push_back(d/info); - - //float diff_z = (dst_z-tz)/(src_z*src_z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 - //residuals.push_back(diff_z); - - - - - - // float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - // float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - //float angle = (tnx*dst_x+tny*dst_y+tnz*dst_z)/sqrt(dst_x*dst_x + dst_y*dst_y + dst_z*dst_z); - //weights.push_back(1-angle); - //weights.push_back(ny*dst_ny); - - - double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); - double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); - - double dist_src = sqrt(src_x*src_x+src_y*src_y+src_z*src_z); - double angle_src = fabs((src_x*src_nx+src_y*src_ny+src_z*src_nz)/dist_src); - weights.push_back(angle_src * angle_dst); - - if(debugg){ - ws.push_back(src_w); - hs.push_back(src_h); - - dst_ws.push_back(dst_w); - dst_hs.push_back(dst_h); - } - } - } - } - } - } - - - - - // DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); - // func->maxp = 1.0; - // func->update_size = true; - // func->zeromean = true; - // func->startreg = 0.0001; - // func->debugg_print = debugg; - // func->bidir = true; - // func->maxnoise = pred; - // func->reset(); - - DistanceWeightFunction2 * func = new DistanceWeightFunction2(); - func->f = THRESHOLD; - func->p = 0.01; - - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); - for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} - func->computeModel(X); - - Eigen::VectorXd W = func->getProbs(X); - - delete func; - - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - oc.score += weight*weights.at(i); - oc.occlusions += ocl*weights.at(i); - } - - //debugg = true; - if(debugg){ - pcl::PointCloud::Ptr scloud (new pcl::PointCloud); - pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); - scloud->points.resize(src_width*src_height); - dcloud->points.resize(dst_width*dst_height); - /* - for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] != 255){continue;} - float z = src_idepth*float(src_depthdata[src_ind]); - if(z > 0){ - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - scloud->points[src_ind].x = tx; - scloud->points[src_ind].y = ty; - scloud->points[src_ind].z = tz+2; - scloud->points[src_ind].r = 0; - scloud->points[src_ind].g = 0; - scloud->points[src_ind].b = 255; - } - } - } - for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - unsigned int dst_ind = dst_h*dst_width+dst_w; - if(true || dst_maskdata[dst_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ - float z = dst_idepth*float(dst_depthdata[dst_ind]); - if(z > 0){// && (dst_w%3 == 0) && (dst_h%3 == 0)){ - float x = (float(dst_w) - dst_cx) * z * dst_ifx; - float y = (float(dst_h) - dst_cy) * z * dst_ify; - dcloud->points[dst_ind].x = x; - dcloud->points[dst_ind].y = y; - dcloud->points[dst_ind].z = z+2; - dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; - dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; - dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; - if(dst_maskdata[dst_ind] == 255){ - dcloud->points[dst_ind].r = 255; - dcloud->points[dst_ind].g = 000; - dcloud->points[dst_ind].b = 255; - } - } - } - } - } - viewer->removeAllPointClouds(); - //printf("%i showing results\n",__LINE__); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - viewer->spin(); - viewer->removeAllPointClouds(); - */ - /* - for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - int dst_ind = dst_h*dst_width+dst_w; - dcloud->points[dst_ind].x = 0; - dcloud->points[dst_ind].y = 0; - dcloud->points[dst_ind].z = 0; - } - } - - - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - if(debugg){ - int w = ws[i]; - int h = hs[i]; - unsigned int src_ind = h * src_width + w; - if(ocl > 0.01 || weight > 0.01){ - scloud->points[src_ind].r = 255.0*ocl; - scloud->points[src_ind].g = 0; - scloud->points[src_ind].b = 0; - }else{ - scloud->points[src_ind].x = 0; - scloud->points[src_ind].y = 0; - scloud->points[src_ind].z = 0; - } - } - } - - viewer->removeAllPointClouds(); - //printf("%i showing results\n",__LINE__); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - viewer->spin(); - viewer->removeAllPointClouds(); - - for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] != 255){continue;} - float z = src_idepth*float(src_depthdata[src_ind]); - if(z > 0){ - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - scloud->points[src_ind].x = tx; - scloud->points[src_ind].y = ty; - scloud->points[src_ind].z = tz+2; - scloud->points[src_ind].r = 0; - scloud->points[src_ind].g = 0; - scloud->points[src_ind].b = 255; - } - } - } - for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - int dst_ind = dst_h*dst_width+dst_w; - if(true || dst_maskdata[dst_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ - float z = dst_idepth*float(dst_depthdata[dst_ind]); - if(z > 0){// && (dst_w%3 == 0) && (dst_h%3 == 0)){ - float x = (float(dst_w) - dst_cx) * z * dst_ifx; - float y = (float(dst_h) - dst_cy) * z * dst_ify; - dcloud->points[dst_ind].x = x; - dcloud->points[dst_ind].y = y; - dcloud->points[dst_ind].z = z+2; - dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; - dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; - dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; - if(dst_maskdata[dst_ind] == 255){ - dcloud->points[dst_ind].r = 255; - dcloud->points[dst_ind].g = 000; - dcloud->points[dst_ind].b = 255; - } - } - } - } - } - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - if(debugg){ - int w = ws[i]; - int h = hs[i]; - unsigned int src_ind = h * src_width + w; - if(ocl > 0.01 || weight > 0.01){ - scloud->points[src_ind].r = 255.0*ocl;//*weights.at(i); - scloud->points[src_ind].g = 255.0*weight;//*weights.at(i); - scloud->points[src_ind].b = 0; - }else{ - scloud->points[src_ind].x = 0; - scloud->points[src_ind].y = 0; - scloud->points[src_ind].z = 0; - } - } - } - - viewer->removeAllPointClouds(); - - //printf("%i showing results\n",__LINE__); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - viewer->spin(); - viewer->removeAllPointClouds(); + //printf("%i showing results\n",__LINE__); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); */ - /* - for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] != 255){continue;} - float z = src_idepth*float(src_depthdata[src_ind]); - if(z > 0){ - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; - scloud->points[src_ind].x = x; - scloud->points[src_ind].y = y; - scloud->points[src_ind].z = z; - scloud->points[src_ind].r = 0; - scloud->points[src_ind].g = 0; - scloud->points[src_ind].b = 0; - } - } - } + /* + for(unsigned int src_w = 0; src_w < src_width-1; src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] != 255){continue;} + float z = src_idepth*float(src_depthdata[src_ind]); + if(z > 0){ + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + scloud->points[src_ind].x = x; + scloud->points[src_ind].y = y; + scloud->points[src_ind].z = z; + scloud->points[src_ind].r = 0; + scloud->points[src_ind].g = 0; + scloud->points[src_ind].b = 0; + } + } + } */ - for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - unsigned int dst_ind = dst_h*dst_width+dst_w; - float z = dst_idepth*float(dst_depthdata[dst_ind]); - if(z > 0){ - float x = (float(dst_w) - dst_cx) * z * dst_ifx; - float y = (float(dst_h) - dst_cy) * z * dst_ify; - dcloud->points[dst_ind].x = x; - dcloud->points[dst_ind].y = y; - dcloud->points[dst_ind].z = z+2; - dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; - dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; - dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; - if(false && dst_maskdata[dst_ind] == 255){ - dcloud->points[dst_ind].r = 255; - dcloud->points[dst_ind].g = 000; - dcloud->points[dst_ind].b = 255; - } - } - } - } - // viewer->removeAllPointClouds(); - // //printf("%i showing results\n",__LINE__); - // viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - // viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - - viewer->removeAllShapes(); - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - if(debugg){ - int w = ws[i]; - int h = hs[i]; - unsigned int src_ind = h * src_width + w; - - int dst_w = dst_ws[i]; - int dst_h = dst_hs[i]; - unsigned int dst_ind = dst_h * src_width + dst_w; - if(ocl > 0.01 || weight > 0.01){ - float z = src_idepth*float(src_depthdata[src_ind]); - float x = (float(w) - src_cx) * z * src_ifx; - float y = (float(h) - src_cy) * z * src_ify; - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - scloud->points[src_ind].x = tx; - scloud->points[src_ind].y = ty; - scloud->points[src_ind].z = tz+2; - - scloud->points[src_ind].r = 255.0*ocl*weights.at(i); - scloud->points[src_ind].g = 255.0*weight*weights.at(i); - scloud->points[src_ind].b = 0; - - char buf [1024]; - sprintf(buf,"line%i",i); - viewer->addLine (scloud->points[src_ind], dcloud->points[dst_ind],buf); - }else{ - scloud->points[src_ind].x = 0; - scloud->points[src_ind].y = 0; - scloud->points[src_ind].z = 0; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud"); - - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - viewer->spin(); - viewer->removeAllPointClouds(); - - /* - for(unsigned int i = 0; i < residuals.size(); i++){ - float r = residuals[i]; - float weight = W(i); - float ocl = 0; - if(r > 0){ocl += 1-weight;} - if(debugg){ - int w = ws[i]; - int h = hs[i]; - unsigned int src_ind = h * src_width + w; - if(ocl > 0.01 || weight > 0.01){ - scloud->points[src_ind].r = 255.0*(1-weights.at(i)); - scloud->points[src_ind].g = 255.0*weights.at(i); - scloud->points[src_ind].b = 0; - }else{ - scloud->points[src_ind].x = 0; - scloud->points[src_ind].y = 0; - scloud->points[src_ind].z = 0; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->spin(); - viewer->removeAllPointClouds(); + for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + unsigned int dst_ind = dst_h*dst_width+dst_w; + float z = dst_idepth*float(dst_depthdata[dst_ind]); + if(z > 0){ + float x = (float(dst_w) - dst_cx) * z * dst_ifx; + float y = (float(dst_h) - dst_cy) * z * dst_ify; + dcloud->points[dst_ind].x = x; + dcloud->points[dst_ind].y = y; + dcloud->points[dst_ind].z = z+2; + dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; + dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; + dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; + if(false && dst_maskdata[dst_ind] == 255){ + dcloud->points[dst_ind].r = 255; + dcloud->points[dst_ind].g = 000; + dcloud->points[dst_ind].b = 255; + } + } + } + } + // viewer->removeAllPointClouds(); + // //printf("%i showing results\n",__LINE__); + // viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + // viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + + viewer->removeAllShapes(); + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + int w = ws[i]; + int h = hs[i]; + unsigned int src_ind = h * src_width + w; + + int dst_w = dst_ws[i]; + int dst_h = dst_hs[i]; + unsigned int dst_ind = dst_h * src_width + dst_w; + if(ocl > 0.01 || weight > 0.01){ + float z = src_idepth*float(src_depthdata[src_ind]); + float x = (float(w) - src_cx) * z * src_ifx; + float y = (float(h) - src_cy) * z * src_ify; + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + scloud->points[src_ind].x = tx; + scloud->points[src_ind].y = ty; + scloud->points[src_ind].z = tz+2; + + scloud->points[src_ind].r = 255.0*ocl*weights.at(i); + scloud->points[src_ind].g = 255.0*weight*weights.at(i); + scloud->points[src_ind].b = 0; + + char buf [1024]; + sprintf(buf,"line%i",i); + viewer->addLine (scloud->points[src_ind], dcloud->points[dst_ind],buf); + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 7, "scloud"); + + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + + /* + for(unsigned int i = 0; i < residuals.size(); i++){ + float r = residuals[i]; + float weight = W(i); + float ocl = 0; + if(r > 0){ocl += 1-weight;} + if(debugg){ + int w = ws[i]; + int h = hs[i]; + unsigned int src_ind = h * src_width + w; + if(ocl > 0.01 || weight > 0.01){ + scloud->points[src_ind].r = 255.0*(1-weights.at(i)); + scloud->points[src_ind].g = 255.0*weights.at(i); + scloud->points[src_ind].b = 0; + }else{ + scloud->points[src_ind].x = 0; + scloud->points[src_ind].y = 0; + scloud->points[src_ind].z = 0; + } + } + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->spin(); + viewer->removeAllPointClouds(); */ - } - /* - if(debugg){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - cv::namedWindow("debuggimage", cv::WINDOW_AUTOSIZE ); - cv::imshow( "debuggimage", debugg_img ); - //cv::waitKey(30); - viewer->spin(); - viewer->removeAllPointClouds(); - } - */ - return oc; + } + /* + if(debugg){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + cv::namedWindow("debuggimage", cv::WINDOW_AUTOSIZE ); + cv::imshow( "debuggimage", debugg_img ); + //cv::waitKey(30); + viewer->spin(); + viewer->removeAllPointClouds(); + } + */ + return oc; } using namespace std; using namespace Eigen; vector > ModelUpdater::computeAllOcclusionScores(RGBDFrame * src, cv::Mat src_mask, RGBDFrame * dst, cv::Mat dst_mask,Eigen::Matrix4d p, bool debugg){ - unsigned char * src_maskdata = (unsigned char *)(src_mask.data); - unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(src->depth.data); - float * src_normalsdata = (float *)(src->normals.data); - - unsigned char * dst_maskdata = (unsigned char *)(dst_mask.data); - unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); - float * dst_normalsdata = (float *)(dst->normals.data); - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - Camera * src_camera = src->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - const int * src_labels = src->labels; - const int src_nr_labels = src->nr_labels; - - Camera * dst_camera = dst->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - const int * dst_labels = dst->labels; - const int dst_nr_labels = src->nr_labels; - - vector< vector< OcclusionScore > > all_scores; - all_scores.resize(src_nr_labels); - for(int i = 0; i < src_nr_labels; i++){ - all_scores[i].resize(dst_nr_labels); - } - - std::vector< std::vector< std::vector > > all_residuals; - all_residuals.resize(src_nr_labels); - for(int i = 0; i < src_nr_labels; i++){ - all_residuals[i].resize(dst_nr_labels); - } - - for(unsigned int src_w = 0; src_w < src_width; src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ - float z = src_idepth*float(src_depthdata[src_ind]); - float nx = src_normalsdata[3*src_ind+0]; - - if(z > 0 && nx != 2){ - float ny = src_normalsdata[3*src_ind+1]; - float nz = src_normalsdata[3*src_ind+2]; - - float x = (float(src_w) - src_cx) * z * src_ifx; - float y = (float(src_h) - src_cy) * z * src_ify; - - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - - float tnx = m00*nx + m01*ny + m02*nz; - float tny = m10*nx + m11*ny + m12*nz; - float tnz = m20*nx + m21*ny + m22*nz; - - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - if(dst_z > 0){ - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - float diff_z2 = tnx*(dst_x-tx)+tny*(dst_y-ty)+tnz*(dst_z-tz); - - float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 - if(diff_z < 0 && diff_z2 > 0){diff_z2 *= -1;} - if(diff_z > 0 && diff_z2 < 0){diff_z2 *= -1;} - diff_z2 /= (z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 - - int src_label = src_labels[src_ind]; - int dst_label = dst_labels[dst_ind]; - all_residuals[src_label][dst_label].push_back(diff_z2); - } - } - } - } - } - } - - DistanceWeightFunction2PPR * func = new DistanceWeightFunction2PPR(); - func->update_size = true; - func->startreg = 0.00001; - func->debugg_print = true; - func->reset(); - - - delete func; - - for(int i = 0; i < src_nr_labels; i++){ - for(int j = 0; j < dst_nr_labels; i++){ - std::vector & resi = all_residuals[i][j]; - OcclusionScore score; - for(unsigned int k = 0; k < resi.size(); k++){ - float r = resi[k]; - float weight = 1; - if(fabs(r) > 0.0005){weight = 0;}//Replace with PPR - - float ocl = 0; - if(r > 0){ocl += 1-weight;} - - score.score += weight; - score.occlusions += ocl; - } - all_scores[i][j] = score; - } - } - return all_scores; + unsigned char * src_maskdata = (unsigned char *)(src_mask.data); + unsigned char * src_rgbdata = (unsigned char *)(src->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(src->depth.data); + float * src_normalsdata = (float *)(src->normals.data); + + unsigned char * dst_maskdata = (unsigned char *)(dst_mask.data); + unsigned char * dst_rgbdata = (unsigned char *)(dst->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(dst->depth.data); + float * dst_normalsdata = (float *)(dst->normals.data); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + Camera * src_camera = src->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + const int * src_labels = src->labels; + const int src_nr_labels = src->nr_labels; + + Camera * dst_camera = dst->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + const int * dst_labels = dst->labels; + const int dst_nr_labels = src->nr_labels; + + vector< vector< OcclusionScore > > all_scores; + all_scores.resize(src_nr_labels); + for(int i = 0; i < src_nr_labels; i++){ + all_scores[i].resize(dst_nr_labels); + } + + std::vector< std::vector< std::vector > > all_residuals; + all_residuals.resize(src_nr_labels); + for(int i = 0; i < src_nr_labels; i++){ + all_residuals[i].resize(dst_nr_labels); + } + + for(unsigned int src_w = 0; src_w < src_width; src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + if(src_maskdata[src_ind] == 255){// && p.z > 0 && !isnan(p.normal_x)){ + float z = src_idepth*float(src_depthdata[src_ind]); + float nx = src_normalsdata[3*src_ind+0]; + + if(z > 0 && nx != 2){ + float ny = src_normalsdata[3*src_ind+1]; + float nz = src_normalsdata[3*src_ind+2]; + + float x = (float(src_w) - src_cx) * z * src_ifx; + float y = (float(src_h) - src_cy) * z * src_ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + float tnx = m00*nx + m01*ny + m02*nz; + float tny = m10*nx + m11*ny + m12*nz; + float tnz = m20*nx + m21*ny + m22*nz; + + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + if(dst_z > 0){ + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + float diff_z2 = tnx*(dst_x-tx)+tny*(dst_y-ty)+tnz*(dst_z-tz); + + float diff_z = (dst_z-tz)/(z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 + if(diff_z < 0 && diff_z2 > 0){diff_z2 *= -1;} + if(diff_z > 0 && diff_z2 < 0){diff_z2 *= -1;} + diff_z2 /= (z*z+dst_z*dst_z);//if tz < dst_z then tz infront and diff_z > 0 + + int src_label = src_labels[src_ind]; + int dst_label = dst_labels[dst_ind]; + all_residuals[src_label][dst_label].push_back(diff_z2); + } + } + } + } + } + } + + DistanceWeightFunction2PPR * func = new DistanceWeightFunction2PPR(); + func->update_size = true; + func->startreg = 0.00001; + func->debugg_print = true; + func->reset(); + + + delete func; + + for(int i = 0; i < src_nr_labels; i++){ + for(int j = 0; j < dst_nr_labels; i++){ + std::vector & resi = all_residuals[i][j]; + OcclusionScore score; + for(unsigned int k = 0; k < resi.size(); k++){ + float r = resi[k]; + float weight = 1; + if(fabs(r) > 0.0005){weight = 0;}//Replace with PPR + + float ocl = 0; + if(r > 0){ocl += 1-weight;} + + score.score += weight; + score.occlusions += ocl; + } + all_scores[i][j] = score; + } + } + return all_scores; } vector > ModelUpdater::getOcclusionScores(std::vector current_poses, std::vector current_frames, std::vector current_modelmasks, bool debugg_scores, double speedup){ - //printf("getOcclusionScores\n"); - - long total_points = 0; - for(unsigned int i = 0; i < current_frames.size(); i++){total_points+=current_modelmasks[i]->testw.size();} - int step = std::max(long(1),long(speedup*total_points*long(current_frames.size()))/long(50000000)); - - // printf("total_points: %i\n",total_points); - // printf("current_frames.size(): %i\n",current_frames.size()); - // printf("ratio: %f\n",double(total_points*long(current_frames.size()))/double(50000000)); - printf("step: %i\n",step); - - vector > occlusionScores; - occlusionScores.resize(current_frames.size()); - for(unsigned int i = 0; i < current_frames.size(); i++){occlusionScores[i].resize(current_frames.size());} - - int max_points = step;//100000.0/double(current_frames.size()*(current_frames.size()-1)); - //float occlusion_penalty = 10.0f; - std::vector > scores; - scores.resize(occlusionScores.size()); - for(unsigned int i = 0; i < occlusionScores.size(); i++){scores[i].resize(occlusionScores.size());} - - bool lock = false; - for(unsigned int i = 0; i < current_frames.size(); i++){ - scores[i][i] = 0; - for(unsigned int j = i+1; j < current_frames.size(); j++){ - //printf("scores %i %i\n",i,j); - if(lock && current_modelmasks[j]->sweepid == current_modelmasks[i]->sweepid && current_modelmasks[j]->sweepid != -1){ - occlusionScores[i][j].score = 999999; - occlusionScores[i][j].occlusions = 0; - occlusionScores[j][i].score = 999999; - occlusionScores[j][i].occlusions = 0; - }else{ - Eigen::Matrix4d relative_pose = current_poses[i].inverse() * current_poses[j]; - occlusionScores[j][i] = computeOcclusionScore(current_frames[j], current_modelmasks[j],current_frames[i], current_modelmasks[i], relative_pose,max_points,debugg_scores); - occlusionScores[i][j] = computeOcclusionScore(current_frames[i], current_modelmasks[i],current_frames[j], current_modelmasks[j], relative_pose.inverse(),max_points,debugg_scores); - //printf("scores: %i %i -> occlusion_penalty: %f -> (%f %f) and (%f %f) -> %f \n",i,j,occlusion_penalty,occlusionScores[i][j].score,occlusionScores[i][j].occlusions,occlusionScores[j][i].score,occlusionScores[j][i].occlusions,occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions)); - } - scores[i][j] = occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions); - scores[j][i] = scores[i][j]; - } - } - return occlusionScores; + //printf("getOcclusionScores\n"); + + long total_points = 0; + for(unsigned int i = 0; i < current_frames.size(); i++){total_points+=current_modelmasks[i]->testw.size();} + int step = std::max(long(1),long(speedup*total_points*long(current_frames.size()))/long(50000000)); + + // printf("total_points: %i\n",total_points); + // printf("current_frames.size(): %i\n",current_frames.size()); + // printf("ratio: %f\n",double(total_points*long(current_frames.size()))/double(50000000)); + printf("step: %i\n",step); + + vector > occlusionScores; + occlusionScores.resize(current_frames.size()); + for(unsigned int i = 0; i < current_frames.size(); i++){occlusionScores[i].resize(current_frames.size());} + + int max_points = step;//100000.0/double(current_frames.size()*(current_frames.size()-1)); + //float occlusion_penalty = 10.0f; + std::vector > scores; + scores.resize(occlusionScores.size()); + for(unsigned int i = 0; i < occlusionScores.size(); i++){scores[i].resize(occlusionScores.size());} + + bool lock = false; + for(unsigned int i = 0; i < current_frames.size(); i++){ + scores[i][i] = 0; + for(unsigned int j = i+1; j < current_frames.size(); j++){ + //printf("scores %i %i\n",i,j); + if(lock && current_modelmasks[j]->sweepid == current_modelmasks[i]->sweepid && current_modelmasks[j]->sweepid != -1){ + occlusionScores[i][j].score = 999999; + occlusionScores[i][j].occlusions = 0; + occlusionScores[j][i].score = 999999; + occlusionScores[j][i].occlusions = 0; + }else{ + Eigen::Matrix4d relative_pose = current_poses[i].inverse() * current_poses[j]; + occlusionScores[j][i] = computeOcclusionScore(current_frames[j], current_modelmasks[j],current_frames[i], current_modelmasks[i], relative_pose,max_points,debugg_scores); + occlusionScores[i][j] = computeOcclusionScore(current_frames[i], current_modelmasks[i],current_frames[j], current_modelmasks[j], relative_pose.inverse(),max_points,debugg_scores); + //printf("scores: %i %i -> occlusion_penalty: %f -> (%f %f) and (%f %f) -> %f \n",i,j,occlusion_penalty,occlusionScores[i][j].score,occlusionScores[i][j].occlusions,occlusionScores[j][i].score,occlusionScores[j][i].occlusions,occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions)); + } + scores[i][j] = occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions); + scores[j][i] = scores[i][j]; + } + } + return occlusionScores; } CloudData * ModelUpdater::getCD(std::vector current_poses, std::vector current_frames,std::vector current_masks, int step){return 0;} @@ -4042,17 +3890,17 @@ CloudData * ModelUpdater::getCD(std::vector current_poses, std: void ModelUpdater::computeMassRegistration(std::vector current_poses, std::vector current_frames,std::vector current_masks){} std::vector > ModelUpdater::getScores(std::vector > occlusionScores){//, float occlusion_penalty){ - std::vector > scores; - scores.resize(occlusionScores.size()); - for(unsigned int i = 0; i < occlusionScores.size(); i++){scores[i].resize(occlusionScores.size());} - for(unsigned int i = 0; i < scores.size(); i++){ - scores[i][i] = 0; - for(unsigned int j = i+1; j < scores.size(); j++){ - scores[i][j] = occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions); - scores[j][i] = scores[i][j]; - } - } - return scores; + std::vector > scores; + scores.resize(occlusionScores.size()); + for(unsigned int i = 0; i < occlusionScores.size(); i++){scores[i].resize(occlusionScores.size());} + for(unsigned int i = 0; i < scores.size(); i++){ + scores[i][i] = 0; + for(unsigned int j = i+1; j < scores.size(); j++){ + scores[i][j] = occlusionScores[i][j].score+occlusionScores[j][i].score - occlusion_penalty*(occlusionScores[i][j].occlusions+occlusionScores[j][i].occlusions); + scores[j][i] = scores[i][j]; + } + } + return scores; } } From 8230a07d2fa60ec33505af0bfd8665ba5dda1750 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 29 Aug 2016 14:18:29 +0200 Subject: [PATCH 078/255] fire drill save --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 590 +++++----- .../include/modelupdater/ModelUpdater.h | 2 +- .../src/modelupdater/ModelUpdater.cpp | 1002 +++++++++++------ 3 files changed, 943 insertions(+), 651 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 03ddb421..34887594 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -3,360 +3,360 @@ namespace quasimodo_brain { double getTime(){ - struct timeval start1; - gettimeofday(&start1, NULL); - return double(start1.tv_sec+(start1.tv_usec/1000000.0)); + struct timeval start1; + gettimeofday(&start1, NULL); + return double(start1.tv_sec+(start1.tv_usec/1000000.0)); } reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg){ - reglib::Model * model = new reglib::Model(); - - for(unsigned int i = 0; i < msg.local_poses.size(); i++){ - sensor_msgs::CameraInfo camera = msg.frames[i].camera; - ros::Time capture_time = msg.frames[i].capture_time; - geometry_msgs::Pose pose = msg.frames[i].pose; - - cv_bridge::CvImagePtr rgb_ptr; - try{ rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat rgb = rgb_ptr->image; - - cv_bridge::CvImagePtr depth_ptr; - try{ depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat depth = depth_ptr->image; - - Eigen::Affine3d epose; - tf::poseMsgToEigen(pose, epose); - - reglib::Camera * cam = new reglib::Camera(); - if(camera.K[0] > 0){ - cam->fx = camera.K[0]; - cam->fy = camera.K[4]; - cam->cx = camera.K[2]; - cam->cy = camera.K[5]; - } - - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); - model->frames.push_back(frame); - - geometry_msgs::Pose pose1 = msg.local_poses[i]; - Eigen::Affine3d epose1; - tf::poseMsgToEigen(pose1, epose1); - model->relativeposes.push_back(epose1.matrix()); - - cv_bridge::CvImagePtr mask_ptr; - try{ mask_ptr = cv_bridge::toCvCopy(msg.masks[i], sensor_msgs::image_encodings::MONO8);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat mask = mask_ptr->image; - - model->modelmasks.push_back(new reglib::ModelMask(mask)); - } - model->recomputeModelPoints(); - return model; + reglib::Model * model = new reglib::Model(); + + for(unsigned int i = 0; i < msg.local_poses.size(); i++){ + sensor_msgs::CameraInfo camera = msg.frames[i].camera; + ros::Time capture_time = msg.frames[i].capture_time; + geometry_msgs::Pose pose = msg.frames[i].pose; + + cv_bridge::CvImagePtr rgb_ptr; + try{ rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat rgb = rgb_ptr->image; + + cv_bridge::CvImagePtr depth_ptr; + try{ depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat depth = depth_ptr->image; + + Eigen::Affine3d epose; + tf::poseMsgToEigen(pose, epose); + + reglib::Camera * cam = new reglib::Camera(); + if(camera.K[0] > 0){ + cam->fx = camera.K[0]; + cam->fy = camera.K[4]; + cam->cx = camera.K[2]; + cam->cy = camera.K[5]; + } + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); + model->frames.push_back(frame); + + geometry_msgs::Pose pose1 = msg.local_poses[i]; + Eigen::Affine3d epose1; + tf::poseMsgToEigen(pose1, epose1); + model->relativeposes.push_back(epose1.matrix()); + + cv_bridge::CvImagePtr mask_ptr; + try{ mask_ptr = cv_bridge::toCvCopy(msg.masks[i], sensor_msgs::image_encodings::MONO8);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat mask = mask_ptr->image; + + model->modelmasks.push_back(new reglib::ModelMask(mask)); + } + model->recomputeModelPoints(); + return model; } void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp){ - int startsize = msg.local_poses.size(); - msg.local_poses.resize(startsize+model->relativeposes.size()); - msg.frames.resize(startsize+model->frames.size()); - msg.masks.resize(startsize+model->modelmasks.size()); - for(unsigned int i = 0; i < model->relativeposes.size(); i++){ - geometry_msgs::Pose pose1; - tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i])*rp, pose1); - geometry_msgs::Pose pose2; - tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose)*rp, pose2); - cv_bridge::CvImage rgbBridgeImage; - rgbBridgeImage.image = model->frames[i]->rgb; - rgbBridgeImage.encoding = "bgr8"; - cv_bridge::CvImage depthBridgeImage; - depthBridgeImage.image = model->frames[i]->depth; - depthBridgeImage.encoding = "mono16"; - cv_bridge::CvImage maskBridgeImage; - maskBridgeImage.image = model->modelmasks[i]->getMask(); - maskBridgeImage.encoding = "mono8"; - msg.local_poses[startsize+i] = pose1; - msg.frames[startsize+i].capture_time = ros::Time(); - msg.frames[startsize+i].pose = pose2; - msg.frames[startsize+i].frame_id = model->frames[i]->id; - msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); - msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); - msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg());//getMask() - - msg.frames[startsize+i].camera.K[0] = model->frames[i]->camera->fx; - msg.frames[startsize+i].camera.K[4] = model->frames[i]->camera->fy; - msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx; - msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy; - - } - for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ - addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp); - } + int startsize = msg.local_poses.size(); + msg.local_poses.resize(startsize+model->relativeposes.size()); + msg.frames.resize(startsize+model->frames.size()); + msg.masks.resize(startsize+model->modelmasks.size()); + for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + geometry_msgs::Pose pose1; + tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i])*rp, pose1); + geometry_msgs::Pose pose2; + tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose)*rp, pose2); + cv_bridge::CvImage rgbBridgeImage; + rgbBridgeImage.image = model->frames[i]->rgb; + rgbBridgeImage.encoding = "bgr8"; + cv_bridge::CvImage depthBridgeImage; + depthBridgeImage.image = model->frames[i]->depth; + depthBridgeImage.encoding = "mono16"; + cv_bridge::CvImage maskBridgeImage; + maskBridgeImage.image = model->modelmasks[i]->getMask(); + maskBridgeImage.encoding = "mono8"; + msg.local_poses[startsize+i] = pose1; + msg.frames[startsize+i].capture_time = ros::Time(); + msg.frames[startsize+i].pose = pose2; + msg.frames[startsize+i].frame_id = model->frames[i]->id; + msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); + msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); + msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg());//getMask() + + msg.frames[startsize+i].camera.K[0] = model->frames[i]->camera->fx; + msg.frames[startsize+i].camera.K[4] = model->frames[i]->camera->fy; + msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx; + msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy; + + } + for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ + addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp); + } } quasimodo_msgs::model getModelMSG(reglib::Model * model){ - quasimodo_msgs::model msg; - msg.model_id = model->id; - addToModelMSG(msg,model); + quasimodo_msgs::model msg; + msg.model_id = model->id; + addToModelMSG(msg,model); - return msg; + return msg; } std::vector getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms){ - std::vector toRet; - ifstream in(poses_file); - if (!in.is_open()){ - cout<<"ERROR: cannot find poses file "<> temp; - transform(j,k) = temp; - } - } - toRet.push_back(transform); - } - return toRet; + std::vector toRet; + ifstream in(poses_file); + if (!in.is_open()){ + cout<<"ERROR: cannot find poses file "<> temp; + transform(j,k) = temp; + } + } + toRet.push_back(transform); + } + return toRet; } Eigen::Matrix4d getMat(tf::StampedTransform tf){ - //Transform - geometry_msgs::TransformStamped tfstmsg; - tf::transformStampedTFToMsg (tf, tfstmsg); - geometry_msgs::Transform tfmsg = tfstmsg.transform; - geometry_msgs::Pose pose; - pose.orientation = tfmsg.rotation; - pose.position.x = tfmsg.translation.x; - pose.position.y = tfmsg.translation.y; - pose.position.z = tfmsg.translation.z; - Eigen::Affine3d epose; - tf::poseMsgToEigen(pose, epose); - return epose.matrix(); + //Transform + geometry_msgs::TransformStamped tfstmsg; + tf::transformStampedTFToMsg (tf, tfstmsg); + geometry_msgs::Transform tfmsg = tfstmsg.transform; + geometry_msgs::Pose pose; + pose.orientation = tfmsg.rotation; + pose.position.x = tfmsg.translation.x; + pose.position.y = tfmsg.translation.y; + pose.position.z = tfmsg.translation.z; + Eigen::Affine3d epose; + tf::poseMsgToEigen(pose, epose); + return epose.matrix(); } reglib::Model * load_metaroom_model(std::string sweep_xml){ - int slash_pos = sweep_xml.find_last_of("/"); - std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; - printf("folder: %s\n",sweep_folder.c_str()); + int slash_pos = sweep_xml.find_last_of("/"); + std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; + printf("folder: %s\n",sweep_folder.c_str()); - SimpleXMLParser parser; - SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); + SimpleXMLParser parser; + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); - reglib::Model * sweepmodel = 0; + reglib::Model * sweepmodel = 0; - Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); - cout << m2 << endl << endl; + Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); + cout << m2 << endl << endl; - std::vector current_room_frames; - for (size_t i=0; i current_room_frames; + for (size_t i=0; i 8){continue;} - cv::Mat fullmask; - fullmask.create(480,640,CV_8UC1); - unsigned char * maskdata = (unsigned char *)fullmask.data; - for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} + cv::Mat fullmask; + fullmask.create(480,640,CV_8UC1); + unsigned char * maskdata = (unsigned char *)fullmask.data; + for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} - reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS - cam->fx = 532.158936; - cam->fy = 533.819214; - cam->cx = 310.514310; - cam->cy = 236.842039; + reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS + cam->fx = 532.158936; + cam->fy = 533.819214; + cam->cx = 310.514310; + cam->cy = 236.842039; - // cout<<"Intermediate cloud size "<points.size()<points.size()<frames.push_back(frame); - sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose); - sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); - } - } - - //sweepmodel->recomputeModelPoints(); - printf("nr points: %i\n",sweepmodel->points.size()); - - return sweepmodel; + sweepmodel = new reglib::Model(frame,fullmask); + }else{ + sweepmodel->frames.push_back(frame); + sweepmodel->relativeposes.push_back(current_room_frames.front()->pose.inverse() * frame->pose); + sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); + } + } + + //sweepmodel->recomputeModelPoints(); + printf("nr points: %i\n",sweepmodel->points.size()); + + return sweepmodel; } void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ - boost::shared_ptr viewer; + boost::shared_ptr viewer; if(debugg){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); viewer->addCoordinateSystem(0.01); viewer->setBackgroundColor(1.0,1.0,1.0); } - reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); - massregmod->timeout = 1200; - massregmod->viewer = viewer; + reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); + massregmod->timeout = 1200; + massregmod->viewer = viewer; massregmod->visualizationLvl = 0; - massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massregmod->nomask = true; - massregmod->stopval = 0.0001; - - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; - - if(models.size() > 0 && bg->frames.size() > 0){ - std::vector cpmod; - - bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); - - cpmod.push_back(Eigen::Matrix4d::Identity());//,models.front()->relativeposes.front().inverse() * bg->relativeposes.front()); - massregmod->addModel(bg); - printf("bg->points = %i\n",bg->points.size()); - - for(int j = 0; j < models.size(); j++){ - models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - cpmod.push_back(bg->frames.front()->pose.inverse() * models[j]->frames.front()->pose);//bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); - massregmod->addModel(models[j]); - } - - - reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); - for(int j = 0; j < models.size(); j++){ - Eigen::Matrix4d change = mfrmod.poses[j+1];// * cpmod[j+1].inverse(); - - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; - } - - for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ - models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; - } - } - }else if(models.size() > 1){ - std::vector cpmod; - - Eigen::Matrix4d first = Eigen::Matrix4d::Identity(); - //Eigen::Matrix4d first = mod_po_vec.front().front(); - for(int j = 0; j < models.size(); j++){ - models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); - massregmod->addModel(models[j]); - } + massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massregmod->nomask = true; + massregmod->stopval = 0.0001; + + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + if(models.size() > 0 && bg->frames.size() > 0){ + std::vector cpmod; + + bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + + cpmod.push_back(Eigen::Matrix4d::Identity());//,models.front()->relativeposes.front().inverse() * bg->relativeposes.front()); + massregmod->addModel(bg); + printf("bg->points = %i\n",bg->points.size()); + + for(int j = 0; j < models.size(); j++){ + models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + cpmod.push_back(bg->frames.front()->pose.inverse() * models[j]->frames.front()->pose);//bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); + massregmod->addModel(models[j]); + } + + + reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); + for(int j = 0; j < models.size(); j++){ + Eigen::Matrix4d change = mfrmod.poses[j+1];// * cpmod[j+1].inverse(); + + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + } + + for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ + models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; + } + } + }else if(models.size() > 1){ + std::vector cpmod; + + Eigen::Matrix4d first = Eigen::Matrix4d::Identity(); + //Eigen::Matrix4d first = mod_po_vec.front().front(); + for(int j = 0; j < models.size(); j++){ + models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); + massregmod->addModel(models[j]); + } + + reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); + for(int j = 0; j < models.size(); j++){ + Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); + + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + } + + for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ + models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; + } + } + } - reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); - for(int j = 0; j < models.size(); j++){ - Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); + delete massregmod; - for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ - models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; - } + std::vector bgcp; + std::vector bgcf; + std::vector bgmask; + for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ + bgcp.push_back(bg->relativeposes[k]); + bgcf.push_back(bg->frames[k]); + bgmask.push_back(bg->modelmasks[k]->getMask()); + } - for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ - models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; - } - } - } + for(int j = 0; j < models.size(); j++){ + reglib::Model * model = models[j]; - delete massregmod; + std::vector masks; + for(unsigned int i = 0; i < model->frames.size(); i++){ + reglib::RGBDFrame * frame = model->frames[i]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + masks.push_back(mask); + } - std::vector bgcp; - std::vector bgcf; - std::vector bgmask; - for(unsigned int k = 0; k < bg->relativeposes.size(); k++){ - bgcp.push_back(bg->relativeposes[k]); - bgcf.push_back(bg->frames[k]); - bgmask.push_back(bg->modelmasks[k]->getMask()); - } - - for(int j = 0; j < models.size(); j++){ - reglib::Model * model = models[j]; - - std::vector masks; - for(unsigned int i = 0; i < model->frames.size(); i++){ - reglib::RGBDFrame * frame = model->frames[i]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} - masks.push_back(mask); - } - - //mu->computeMovingDynamicStatic(bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions - // mu->computeMovingDynamicStatic(bgcp,bgcf,mod_po_vec[j],mod_fr_vec[j],debugg);//Determine self occlusions + mu->computeMovingDynamicStatic(bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions + // mu->computeMovingDynamicStatic(bgcp,bgcf,mod_po_vec[j],mod_fr_vec[j],debugg);//Determine self occlusions std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,debugg);//Determine self occlusions std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,debugg);//Determine external occlusions - std::vector dynamic_masks; - for(unsigned int i = 0; i < model->frames.size(); i++){ - reglib::RGBDFrame * frame = model->frames[i]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} - - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); - unsigned char * externalmaskdata = (unsigned char *)(external_masks[i].data); - for(unsigned int k = 0; k < cam->height * cam->width;k++){ - if(externalmaskdata[k] == 0 && internalmaskdata[k] != 0 ){ - maskdata[k] = 255; - }else{ - maskdata[k] = 0; - } - } - - dynamic_masks.push_back(mask); - - // cv::imshow( "rgb", frame->rgb ); - // cv::imshow( "internal_masks", internal_masks[i] ); - // cv::imshow( "externalmask", external_masks[i] ); - // cv::imshow( "dynamic_mask", dynamic_masks[i] ); - // cv::waitKey(0); - } - - internal.push_back(internal_masks); - external.push_back(external_masks); - dynamic.push_back(dynamic_masks); - } - - delete reg; - delete mu; + std::vector dynamic_masks; + for(unsigned int i = 0; i < model->frames.size(); i++){ + reglib::RGBDFrame * frame = model->frames[i]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[i].data); + for(unsigned int k = 0; k < cam->height * cam->width;k++){ + if(externalmaskdata[k] == 0 && internalmaskdata[k] != 0 ){ + maskdata[k] = 255; + }else{ + maskdata[k] = 0; + } + } + + dynamic_masks.push_back(mask); + + // cv::imshow( "rgb", frame->rgb ); + // cv::imshow( "internal_masks", internal_masks[i] ); + // cv::imshow( "externalmask", external_masks[i] ); + // cv::imshow( "dynamic_mask", dynamic_masks[i] ); + // cv::waitKey(0); + } + + internal.push_back(internal_masks); + external.push_back(external_masks); + dynamic.push_back(dynamic_masks); + } + + delete reg; + delete mu; } } diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 53353979..d3030d3f 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -92,7 +92,7 @@ namespace reglib virtual void getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2); virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); - virtual void getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, double debugg); + virtual void getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength,double debugg = false); virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks); virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg = false); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 9aa08789..56657807 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1638,9 +1638,9 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d if(debugg){ -// cv::namedWindow( "src_det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_det_dilate", frame1->det_dilate); -// cv::namedWindow( "dst_det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "dst_det_dilate", frame2->det_dilate); -// cv::waitKey(0); + // cv::namedWindow( "src_det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "src_det_dilate", frame1->det_dilate); + // cv::namedWindow( "dst_det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "dst_det_dilate", frame2->det_dilate); + // cv::waitKey(0); src_cloud->points.resize(src_width*src_height); for(unsigned int src_w = 0; src_w < src_width;src_w++){ @@ -1665,8 +1665,8 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d point.y = ty; point.z = tz; //point.r = 255*src_detdata[src_ind] != 0; -// point.g = 0; -// point.b = 0; + // point.g = 0; + // point.b = 0; point.r = src_detdata[src_ind]; point.g = 0; @@ -1703,10 +1703,10 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d } -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); -// viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); -// viewer->spin(); + // viewer->removeAllPointClouds(); + // viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + // viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); + // viewer->spin(); } int informations = 0; @@ -2122,9 +2122,9 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int edgesdata[3*i+2] = dyc[i]; } -// cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); -// cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); -// cv::waitKey(0); + // cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); + // cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); + // cv::waitKey(0); return probs2; } @@ -2174,7 +2174,7 @@ std::vector doInference(std::vector & prior, std::vector< std::vect return labels; } -void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, double debugg){ +void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); @@ -2323,6 +2323,14 @@ void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * ove float tnz = m20*src_nx + m21*src_ny + m22*src_nz; double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / sqrt(dst_noise*dst_noise + point_noise*point_noise); + + if(offset1 >= 0 && offset2 >= 0){ + if(fabs(d) < 0.02){ + interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); + interframe_connectionStrength[offset1+src_ind].push_back(2); + } + } + double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; if(dst_detdata[dst_ind] != 0){continue;} @@ -2330,7 +2338,15 @@ void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * ove if(fabs(d) < 0.02){//If close, according noises, and angle of the surfaces similar: FUSE if(surface_angle > 0.9){ + + + + overlaps[src_ind] = std::max(overlaps[src_ind],0.9); + if(offset1 >= 0 && offset2 >= 0){ + interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); + interframe_connectionStrength[offset1+src_ind].push_back(-log(1-overlaps[src_ind])); + } // overlaps[src_ind]++; // if(debugg){ //// dst_cloud->points[dst_ind].r = 0; @@ -2374,16 +2390,30 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector prior; + std::vector< std::vector > connectionId; + std::vector< std::vector > connectionStrength; + std::vector< std::vector > interframe_connectionId; + std::vector< std::vector > interframe_connectionStrength; + + prior.resize(tot_nr_pixels); + connectionId.resize(tot_nr_pixels); + connectionStrength.resize(tot_nr_pixels); + interframe_connectionId.resize(tot_nr_pixels); + interframe_connectionStrength.resize(tot_nr_pixels); + int current_point = 0; double * priors = new double[3*tot_nr_pixels]; - double * priors_weight = new double[3*tot_nr_pixels]; - double * estimates = new double[3*tot_nr_pixels]; - double * points = new double[3*tot_nr_pixels]; - double * noise = new double[3*tot_nr_pixels]; - double * colors = new double[3*tot_nr_pixels]; - double * normals = new double[3*tot_nr_pixels]; - + // double * priors_weight = new double[3*tot_nr_pixels]; + // double * estimates = new double[3*tot_nr_pixels]; + // double * points = new double[3*tot_nr_pixels]; + // double * noise = new double[3*tot_nr_pixels]; + // double * colors = new double[3*tot_nr_pixels]; + // double * normals = new double[3*tot_nr_pixels]; + + int frameConnections = 0; std::vector< std::vector< std::vector > > pixel_weights; pcl::PointCloud::Ptr cloud (new pcl::PointCloud); @@ -2418,7 +2448,7 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcp, vector bgcp, vector 0.00000001){frameConnections++;} + if(probs[1][ind] > 0.00000001){frameConnections++;} + + current_point++; + } + } + + Eigen::Matrix4d p = poses[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + for(unsigned int h = 0; h < height;h++){ + for(unsigned int w = 0; w < width;w++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; pcl::PointXYZRGBNormal point; point.x = m00*x + m01*y + m02*z + m03; point.y = m10*x + m11*y + m12*z + m13; point.z = m20*x + m21*y + m22*z + m23; - point.r = p_moving_tot*255.0; - point.g = p_dynamic_tot*255.0; - point.b = p_static_tot*255.0; + point.r = priors[3*(offset+ind)+0]*255.0; + point.g = priors[3*(offset+ind)+1]*255.0; + point.b = priors[3*(offset+ind)+2]*255.0; cloud->points.push_back(point); - /* - priors[3*current_point+0] = p_moving_tot; - priors[3*current_point+1] = p_dynamic_tot; - priors[3*current_point+2] = p_static_tot; - priors_weight[3*current_point+0] = -log(1-p_moving_tot); - priors_weight[3*current_point+1] = -log(1-p_dynamic_tot); - priors_weight[3*current_point+2] = -log(1-p_static_tot); - current_point++; - */ } } @@ -2494,37 +2526,298 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vectorpoints[i].r = estimates[3*i+0]*255.0; - cloud->points[i].g = estimates[3*i+1]*255.0; - cloud->points[i].b = estimates[3*i+2]*255.0; + long interframeConnections = 0; + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + interframeConnections++; } + } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); + printf("frameConnections: %i\n",frameConnections); + printf("interframeConnections: %i\n",interframeConnections); + + double start_inf = getTime(); + gc::Graph * g = new gc::Graph(current_point,frameConnections+interframeConnections); + for(unsigned long i = 0; i < current_point;i++){ + g -> add_node(); + double p_fg = priors[3*i+0]+priors[3*i+1]; + double p_bg = priors[3*i+2]; + if(priors[3*i+0]+priors[3*i+1]+priors[3*i+2] <= 0){ + g -> add_tweights( i, 0, 0 ); + continue; + } + + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + g -> add_tweights( i, weightFG, weightBG ); } - std::vector prior; - prior.resize(current_point); + double maxprob_same = 0.999999; + for(unsigned int i = 0; i < frames.size(); i++){ + int offset = offsets[i]; + Camera * camera = frames[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + std::vector< std::vector > & probs = pixel_weights[i]; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(w > 0 && probs[0][ind] > 0.00000001){ + int other = ind-1; + double p_same = std::min(probs[0][ind],maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + g -> add_edge( ind+offset, other+offset, weight, weight ); + } + + if(h > 0 && probs[1][ind] > 0.00000001){ + int other = ind-width; + double p_same = std::min(probs[1][ind],maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + g -> add_edge( ind+offset, other+offset, weight, weight ); + } + } + } + } + + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + double weight = interframe_connectionStrength[i][j]; + g -> add_edge( i, interframe_connectionId[i][j], weight, weight ); + } + } + g -> maxflow(); + + int dynamic_label = 1; + std::vector labels; + std::vector dyn_ind; + std::vector stat_ind; + labels.resize(current_point); + stat_ind.resize(current_point); + long nr_dynamic = 0; + for(unsigned long i = 0; i < current_point;i++){ + labels[i] = g->what_segment(i); + nr_dynamic += labels[i]==dynamic_label; + if(labels[i]==dynamic_label){ + stat_ind[i] = dyn_ind.size(); + dyn_ind.push_back(i); + } + } + delete g; + + double end_inf = getTime(); + printf("static inference time: %10.10fs\n",end_inf-start_inf); + + + + printf("nr_dynamic: %i\n",nr_dynamic); + long dynamic_frameConnections = 0; + + for(unsigned long i = 0; i < frames.size(); i++){ + int offset = offsets[i]; + Camera * camera = frames[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + std::vector< std::vector > & probs = pixel_weights[i]; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(labels[ind+offset] == dynamic_label){ + if(w > 0 && probs[0][ind] > 0.00000001){ + int other = ind-1; + if(labels[ind+offset] == labels[other+offset]){dynamic_frameConnections++;} + } + + if(h > 0 && probs[1][ind] > 0.00000001){ + int other = ind-width; + if(labels[ind+offset] == labels[other+offset]){dynamic_frameConnections++;} + } + } + } + } + } + + long dynamic_interframeConnections = 0; + for(unsigned long i = 0; i < current_point;i++){ + if(labels[i] == dynamic_label){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + if(labels[interframe_connectionId[i][j]] == labels[i]){dynamic_interframeConnections++;} + } + } + } + + + + + printf("dynamic_frameConnections: %i\n",dynamic_frameConnections); + printf("dynamic_interframeConnections: %i\n",dynamic_interframeConnections); + + start_inf = getTime(); + gc::Graph * dynamic_g = new gc::Graph(nr_dynamic,dynamic_frameConnections+dynamic_interframeConnections); + + for(unsigned long i = 0; i < dyn_ind.size();i++){ + dynamic_g -> add_node(); + double p_fg = priors[3*dyn_ind[i]+0]; + double p_bg = priors[3*dyn_ind[i]+1]; + double norm = p_fg+p_bg; + p_fg /= norm; + p_bg /= norm; + if(norm <= 0){ + dynamic_g -> add_tweights( i, 0, 0 ); + continue; + } + + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + dynamic_g -> add_tweights( i, weightFG, weightBG ); + } + + for(unsigned long i = 0; i < frames.size(); i++){ + int offset = offsets[i]; + Camera * camera = frames[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + std::vector< std::vector > & probs = pixel_weights[i]; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(labels[ind+offset] == dynamic_label){ + if(w > 0 && probs[0][ind] > 0.00000001){ + int other = ind-1; + if(labels[ind+offset] == labels[other+offset]){ + //dynamic_frameConnections++; + double p_same = std::min(probs[0][ind],maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight ); + } + } + + if(h > 0 && probs[1][ind] > 0.00000001){ + int other = ind-width; + if(labels[ind+offset] == labels[other+offset]){ + //dynamic_frameConnections++; + double p_same = std::min(probs[0][ind],maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight ); + } + } + } + } + } + } + + + for(unsigned long i = 0; i < current_point;i++){ + if(labels[i] == dynamic_label){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + if(labels[interframe_connectionId[i][j]] == labels[i]){ + double weight = interframe_connectionStrength[i][j]; + dynamic_g -> add_edge( stat_ind[i], stat_ind[ interframe_connectionId[i][j] ], weight, weight ); + } + } + } + } + + + dynamic_g -> maxflow(); + + end_inf = getTime(); + printf("dynamic inference time: %10.10fs\n",end_inf-start_inf); + + long nr_moving = 0; + for(unsigned long i = 0; i < dyn_ind.size();i++){ + int res = dynamic_g->what_segment(i); + if(res == 1){ + labels[dyn_ind[i]] = 2; + nr_moving++; + } + } + delete dynamic_g; + + + std::vector staticdata; + std::vector dynamicdata; + std::vector movingdata; + + + + pcl::PointCloud::Ptr staticcloud (new pcl::PointCloud); for(unsigned int i = 0; i < current_point; i++){ - prior[i] = estimates[3*i+0]; -// cloud->points[i].g = estimates[3*i+1]*255.0; -// cloud->points[i].b = estimates[3*i+2]*255.0; + if(labels[i] == 0){ + staticdata.push_back(i) + } + + if(labels[i] == 1){ + + } + + if(labels[i] == 2){ + } } +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); +// viewer->spin(); - if(debugg){ - for(unsigned int i = 0; i < current_point; i++){ - cloud->points[i].r = estimates[3*i+0]*255.0; - cloud->points[i].g = (1-estimates[3*i+0])*255.0; +// printf("nr_moving: %i\n",nr_moving); + + + +if(debugg){ + printf("3-class\n"); + for(unsigned int i = 0; i < current_point; i++){ + cloud->points[i].r = priors[3*i+0]*255.0; + cloud->points[i].g = priors[3*i+1]*255.0; + cloud->points[i].b = priors[3*i+2]*255.0; + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + +// printf("2-class\n"); +// for(unsigned int i = 0; i < current_point; i++){ +// cloud->points[i].r = priors[3*i+2]*255.0; +// cloud->points[i].g = (priors[3*i+0]+priors[3*i+1])*255.0; +// cloud->points[i].b = 0; +// } +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); +// viewer->spin(); + + printf("results-class\n"); + for(unsigned int i = 0; i < current_point; i++){ + if(labels[i] == 0){ + cloud->points[i].r = 0; + cloud->points[i].g = 0; + cloud->points[i].b = 255; + } + + if(labels[i] == 1){ + cloud->points[i].r = 0; + cloud->points[i].g = 255; cloud->points[i].b = 0; } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); + if(labels[i] == 2){ + cloud->points[i].r = 255; + cloud->points[i].g = 0; + cloud->points[i].b = 0; + } } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); +} + + + + + + + +//return labels; // // std::vector< std::vector > frame_connectionId; // std::vector< std::vector > frame_connectionStrength; @@ -2541,162 +2834,162 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector improvedglobal_labels = doInference(prior,connectionId,connectionStrength); - delete[] estimates; - delete[] priors_weight; - delete[] priors; - delete[] points; - delete[] colors; - delete[] normals; +// delete[] estimates; +// delete[] priors_weight; +delete[] priors; +// delete[] points; +// delete[] colors; +// delete[] normals; - exit(0); +exit(0); } vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg){ -// -std::vector newmasks; -//for(double test = 0.99; test > 0.00000001; test *= 0.1) -{ -// double maxprob_same = 1-test;//0.99; + // + std::vector newmasks; + //for(double test = 0.99; test > 0.00000001; test *= 0.1) + { + // double maxprob_same = 1-test;//0.99; - double maxprob_same = 0.999999; + double maxprob_same = 0.999999; - double maxprob = 0.7; + double maxprob = 0.7; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud1 (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud1 (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); - pcl::PointCloud::Ptr bgcloud (new pcl::PointCloud); + pcl::PointCloud::Ptr bgcloud (new pcl::PointCloud); - int tot_nr_pixels = 0; - std::vector offsets; - - for(unsigned int i = 0; i < frames.size(); i++){ - offsets.push_back(tot_nr_pixels); - unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; - tot_nr_pixels += nr_pixels; - } + int tot_nr_pixels = 0; + std::vector offsets; - //Graph... - std::vector prior; - std::vector< std::vector > connectionId; - std::vector< std::vector > connectionStrength; - std::vector< std::vector > interframe_connectionId; - std::vector< std::vector > interframe_connectionStrength; + for(unsigned int i = 0; i < frames.size(); i++){ + offsets.push_back(tot_nr_pixels); + unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; + tot_nr_pixels += nr_pixels; + } - prior.resize(tot_nr_pixels); - connectionId.resize(tot_nr_pixels); - connectionStrength.resize(tot_nr_pixels); - interframe_connectionId.resize(tot_nr_pixels); - interframe_connectionStrength.resize(tot_nr_pixels); + //Graph... + std::vector prior; + std::vector< std::vector > connectionId; + std::vector< std::vector > connectionStrength; + std::vector< std::vector > interframe_connectionId; + std::vector< std::vector > interframe_connectionStrength; + prior.resize(tot_nr_pixels); + connectionId.resize(tot_nr_pixels); + connectionStrength.resize(tot_nr_pixels); + interframe_connectionId.resize(tot_nr_pixels); + interframe_connectionStrength.resize(tot_nr_pixels); - for(unsigned int i = 0; i < frames.size(); i++){ - printf("frame: %i\n",i); - int offset = offsets[i]; - RGBDFrame * frame = frames[i]; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned char * detdata = (unsigned char *)(frame->det_dilate.data); - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; + for(unsigned int i = 0; i < frames.size(); i++){ + printf("frame: %i\n",i); + int offset = offsets[i]; + RGBDFrame * frame = frames[i]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned char * detdata = (unsigned char *)(frame->det_dilate.data); + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; - unsigned int nr_pixels = frame->camera->width * frame->camera->height; - double * overlaps = new double[nr_pixels]; - double * occlusions = new double[nr_pixels]; - for(unsigned int j = 0; j < nr_pixels; j++){ - overlaps[j] = 0; - occlusions[j] = 0; - } - std::vector< std::vector > interframe_connectionId_tmp; - std::vector< std::vector > interframe_connectionStrength_tmp; + unsigned int nr_pixels = frame->camera->width * frame->camera->height; + double * overlaps = new double[nr_pixels]; + double * occlusions = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + overlaps[j] = 0; + occlusions[j] = 0; + } - for(unsigned int j = 0; j < bgcp.size(); j++){ - if(frame == bgcf[j]){continue;} - Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); - } + std::vector< std::vector > interframe_connectionId_tmp; + std::vector< std::vector > interframe_connectionStrength_tmp; - for(unsigned int j = 0; j < nr_pixels; j++){ - occlusions[j] = 0; - } - printf("CHECK AGAINST BACKGROUND\n"); - for(unsigned int j = 0; j < cp.size(); j++){ - if(frame == cf[j]){continue;} - Eigen::Matrix4d p = poses[i].inverse() * cp[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, cf[j],mm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); - } - std::vector< std::vector > probs = getImageProbs(frames[i],9); + for(unsigned int j = 0; j < bgcp.size(); j++){ + if(frame == bgcf[j]){continue;} + Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, bgcf[j],bgmm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); + } - std::vector frame_prior; - std::vector< std::vector > frame_connectionId; - std::vector< std::vector > frame_connectionStrength; + for(unsigned int j = 0; j < nr_pixels; j++){ + occlusions[j] = 0; + } + printf("CHECK AGAINST BACKGROUND\n"); + for(unsigned int j = 0; j < cp.size(); j++){ + if(frame == cf[j]){continue;} + Eigen::Matrix4d p = poses[i].inverse() * cp[j]; + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, cf[j],mm[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp,false); + } + std::vector< std::vector > probs = getImageProbs(frames[i],9); - frame_prior.resize(nr_pixels); - frame_connectionId.resize(nr_pixels); - frame_connectionStrength.resize(nr_pixels); + std::vector frame_prior; + std::vector< std::vector > frame_connectionId; + std::vector< std::vector > frame_connectionStrength; + frame_prior.resize(nr_pixels); + frame_connectionId.resize(nr_pixels); + frame_connectionStrength.resize(nr_pixels); - printf("starting partition\n"); - double start_part = getTime(); - unsigned int nr_data = width*height; - for(unsigned int j = 0; j < nr_data;j++){ - if(depthdata[j] == 0){ - frame_prior[j] = -1; - prior[offset+j] = -1; - continue; - } - double p_fg = 0.499999999; + printf("starting partition\n"); + double start_part = getTime(); + unsigned int nr_data = width*height; + for(unsigned int j = 0; j < nr_data;j++){ + if(depthdata[j] == 0){ + frame_prior[j] = -1; + prior[offset+j] = -1; + continue; + } - if(occlusions[j] >= 1){ p_fg = maxprob;} - else if(overlaps[j] >= 1){ p_fg = 0.4;} + double p_fg = 0.499999999; - p_fg = std::max(1-maxprob,std::min(maxprob,p_fg)); + if(occlusions[j] >= 1){ p_fg = maxprob;} + else if(overlaps[j] >= 1){ p_fg = 0.4;} - frame_prior[j] = p_fg; - prior[offset+j] = p_fg; - } + p_fg = std::max(1-maxprob,std::min(maxprob,p_fg)); + frame_prior[j] = p_fg; + prior[offset+j] = p_fg; + } - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - if(w > 0 && w < width-1){ - int other = ind-1; - double p_same = std::min(probs[0][ind],maxprob_same); - double not_p_same = 1-p_same; - double weight = -log(not_p_same); - if(!std::isnan(weight) && weight > 0){ - frame_connectionId[ind].push_back(other); - frame_connectionStrength[ind].push_back(weight); - connectionId[ind+offset].push_back(other+offset); - connectionStrength[ind+offset].push_back(weight); + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(w > 0 && w < width-1){ + int other = ind-1; + double p_same = std::min(probs[0][ind],maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + if(!std::isnan(weight) && weight > 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); + + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } } - } - if(h > 0 && h < height-1){ - int other = ind-width; - double p_same = std::min(probs[1][ind],maxprob_same); - double not_p_same = 1-p_same; - double weight = -log(not_p_same); + if(h > 0 && h < height-1){ + int other = ind-width; + double p_same = std::min(probs[1][ind],maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); - if(!std::isnan(weight) && weight > 0){ - frame_connectionId[ind].push_back(other); - frame_connectionStrength[ind].push_back(weight); + if(!std::isnan(weight) && weight > 0){ + frame_connectionId[ind].push_back(other); + frame_connectionStrength[ind].push_back(weight); - connectionId[ind+offset].push_back(other+offset); - connectionStrength[ind+offset].push_back(weight); + connectionId[ind+offset].push_back(other+offset); + connectionStrength[ind+offset].push_back(weight); + } } - } -/* + /* if(w > 0 && w < width-1){ double ax = 0.5; double bx = 0.5; @@ -2771,51 +3064,28 @@ std::vector newmasks; } } */ + } } - } - double end_part = getTime(); - printf("part time: %10.10fs\n",end_part-start_part); - - //std::vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); - - Eigen::Matrix4d p = poses[i]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + double end_part = getTime(); + printf("part time: %10.10fs\n",end_part-start_part); + //std::vector labels = doInference(frame_prior, frame_connectionId, frame_connectionStrength); - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; + Eigen::Matrix4d p = poses[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - pcl::PointXYZRGBNormal point; - point.x = m00*x + m01*y + m02*z + m03; - point.y = m10*x + m11*y + m12*z + m13; - point.z = m20*x + m21*y + m22*z + m23; - point.b = rgbdata[3*ind+0]; - point.g = rgbdata[3*ind+1]; - point.r = rgbdata[3*ind+2]; - - cloud->points.push_back(point); - } - } - } + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; - bool debugg2 = debugg; - if(debugg2){ for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; @@ -2824,170 +3094,193 @@ std::vector newmasks; float x = (float(w) - cx) * z * ifx; float y = (float(h) - cy) * z * ify; - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; + point.x = m00*x + m01*y + m02*z + m03; + point.y = m10*x + m11*y + m12*z + m13; + point.z = m20*x + m21*y + m22*z + m23; + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; - point.r = 0; - point.g = 255; - point.b = 255; + cloud->points.push_back(point); + } + } + } + + bool debugg2 = debugg; + if(debugg2){ + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; - if(overlaps[ind] >= 1){ point.r = 0; point.g = 255; - point.b = 0; - } - if(occlusions[ind] >= 1){ - point.r = 255; - point.g = 0; - point.b = 0; + point.b = 255; + + if(overlaps[ind] >= 1){ + point.r = 0; + point.g = 255; + point.b = 0; + } + if(occlusions[ind] >= 1){ + point.r = 255; + point.g = 0; + point.b = 0; + } + //cloud->points[ind] = point; + cloud2->points.push_back(point); } - //cloud->points[ind] = point; - cloud2->points.push_back(point); } } } - } - for(unsigned int j = 0; j < nr_pixels; j++){ - occlusions[j] = 0; - } + for(unsigned int j = 0; j < nr_pixels; j++){ + occlusions[j] = 0; + } - for(unsigned int j = 0; j < frames.size(); j++){ - if(frame == frames[j]){continue;} - Eigen::Matrix4d p = poses[i].inverse() * poses[j]; - getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, frames[j],masks[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); - } + for(unsigned int j = 0; j < frames.size(); j++){ + if(frame == frames[j]){continue;} + Eigen::Matrix4d p = poses[i].inverse() * poses[j]; + getDynamicWeights(false,p.inverse(), frame, overlaps, occlusions, frames[j],masks[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); + } - //Time to compute external masks... - delete[] overlaps; - delete[] occlusions; - } + //Time to compute external masks... + delete[] overlaps; + delete[] occlusions; + } - //std::vector global_labels = doInference(prior,connectionId,connectionStrength); + //std::vector global_labels = doInference(prior,connectionId,connectionStrength); - for(unsigned int i = 0; i < interframe_connectionId.size(); i++){ - for(unsigned int j = 0; j < interframe_connectionId[i].size(); j++){ - connectionId[i].push_back(interframe_connectionId[i][j]); - connectionStrength[i].push_back(interframe_connectionStrength[i][j]); + for(unsigned int i = 0; i < interframe_connectionId.size(); i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size(); j++){ + connectionId[i].push_back(interframe_connectionId[i][j]); + connectionStrength[i].push_back(interframe_connectionStrength[i][j]); + } } - } - std::vector improvedglobal_labels = doInference(prior,connectionId,connectionStrength); + std::vector improvedglobal_labels = doInference(prior,connectionId,connectionStrength); - for(unsigned int f = 0; f < frames.size(); f++){ - int offset = offsets[f]; - RGBDFrame * frame = frames[f]; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - // unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + for(unsigned int f = 0; f < frames.size(); f++){ + int offset = offsets[f]; + RGBDFrame * frame = frames[f]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + // unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; - unsigned int nr_pixels = frame->camera->width * frame->camera->height; - - // cv::Mat originalmask; - // originalmask.create(height,width,CV_8UC1); - // unsigned char * originaldata = (unsigned char *)(originalmask.data); - - cv::Mat improvedmask; - improvedmask.create(height,width,CV_8UC1); - unsigned char * improveddata = (unsigned char *)(improvedmask.data); - - // cv::Mat diffmask; - // diffmask.create(height,width,CV_8UC3); - // unsigned char * diffdata = (unsigned char *)(diffmask.data); - - for(unsigned int i = 0; i < nr_pixels;i++){ - //internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); - //originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph::SOURCE)); - improveddata[i] = 255.0*((depthdata[i] == 0) || (improvedglobal_labels[i+offset] == gc::Graph::SOURCE)); - // if(improveddata[i] < originaldata[i]){ - // diffdata[3*i+0] = 0; - // diffdata[3*i+1] = 255; - // diffdata[3*i+2] = 0; - // }else if(improveddata[i] > originaldata[i]){ - // diffdata[3*i+0] = 0; - // diffdata[3*i+1] = 0; - // diffdata[3*i+2] = 255; - // }else{ - // diffdata[3*i+0] = 0; - // diffdata[3*i+1] = 0; - // diffdata[3*i+2] = 0; - // } + unsigned int nr_pixels = frame->camera->width * frame->camera->height; + + // cv::Mat originalmask; + // originalmask.create(height,width,CV_8UC1); + // unsigned char * originaldata = (unsigned char *)(originalmask.data); + + cv::Mat improvedmask; + improvedmask.create(height,width,CV_8UC1); + unsigned char * improveddata = (unsigned char *)(improvedmask.data); + + // cv::Mat diffmask; + // diffmask.create(height,width,CV_8UC3); + // unsigned char * diffdata = (unsigned char *)(diffmask.data); + + for(unsigned int i = 0; i < nr_pixels;i++){ + //internaldata[i] = 255.0*((depthdata[i] == 0) || (g->what_segment(i) == gc::Graph::SOURCE)); + //originaldata[i] = 255.0*((depthdata[i] == 0) || (global_labels[i+offset] == gc::Graph::SOURCE)); + improveddata[i] = 255.0*((depthdata[i] == 0) || (improvedglobal_labels[i+offset] == gc::Graph::SOURCE)); + // if(improveddata[i] < originaldata[i]){ + // diffdata[3*i+0] = 0; + // diffdata[3*i+1] = 255; + // diffdata[3*i+2] = 0; + // }else if(improveddata[i] > originaldata[i]){ + // diffdata[3*i+0] = 0; + // diffdata[3*i+1] = 0; + // diffdata[3*i+2] = 255; + // }else{ + // diffdata[3*i+0] = 0; + // diffdata[3*i+1] = 0; + // diffdata[3*i+2] = 0; + // } + } + + // frame->show(false); + // //cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); + // //cv::imshow( "Original", originalmask ); + // cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); + // cv::imshow( "Improved", improvedmask ); + // //cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); + // //cv::imshow( "Difference", diffmask ); + // cv::waitKey(0); + newmasks.push_back(improvedmask); } -// frame->show(false); -// //cv::namedWindow( "Original", cv::WINDOW_AUTOSIZE ); -// //cv::imshow( "Original", originalmask ); -// cv::namedWindow( "Improved", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "Improved", improvedmask ); -// //cv::namedWindow( "Difference", cv::WINDOW_AUTOSIZE ); -// //cv::imshow( "Difference", diffmask ); -// cv::waitKey(0); - newmasks.push_back(improvedmask); - } + if(debugg){ - if(debugg){ + for(unsigned int i = 0; i < bgcf.size(); i++){ + RGBDFrame * frame = bgcf[i]; + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - for(unsigned int i = 0; i < bgcf.size(); i++){ - RGBDFrame * frame = bgcf[i]; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; + Eigen::Matrix4d p = bgcp[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - Eigen::Matrix4d p = bgcp[i]; - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; + point.b = rgbdata[3*ind+0]; + point.g = rgbdata[3*ind+1]; + point.r = rgbdata[3*ind+2]; - point.b = rgbdata[3*ind+0]; - point.g = rgbdata[3*ind+1]; - point.r = rgbdata[3*ind+2]; - - bgcloud->points.push_back(point); + bgcloud->points.push_back(point); + } } } } - } - /* + /* viewer->removeAllPointClouds(); viewer->addPointCloud (cloud1, pcl::visualization::PointCloudColorHandlerRGBField(cloud1), "scloud"); viewer->spin(); @@ -2996,15 +3289,15 @@ std::vector newmasks; viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); viewer->spin(); */ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "scloud"); - //viewer->addPointCloud (bgcloud, pcl::visualization::PointCloudColorHandlerRGBField(bgcloud), "bgcloud"); - viewer->spin(); + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "scloud"); + //viewer->addPointCloud (bgcloud, pcl::visualization::PointCloudColorHandlerRGBField(bgcloud), "bgcloud"); + viewer->spin(); - viewer->removeAllPointClouds(); + viewer->removeAllPointClouds(); + } } -} return newmasks; } @@ -3866,7 +4159,6 @@ vector > ModelUpdater::getOcclusionScores(std::vector< for(unsigned int i = 0; i < current_frames.size(); i++){ scores[i][i] = 0; for(unsigned int j = i+1; j < current_frames.size(); j++){ - //printf("scores %i %i\n",i,j); if(lock && current_modelmasks[j]->sweepid == current_modelmasks[i]->sweepid && current_modelmasks[j]->sweepid != -1){ occlusionScores[i][j].score = 999999; occlusionScores[i][j].occlusions = 0; From 645a0a6f0daf1639ea1f56dc07775e50a79cd704 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 29 Aug 2016 17:15:15 +0200 Subject: [PATCH 079/255] ... --- quasimodo/quasimodo_brain/CMakeLists.txt | 2 +- .../src/ModelDatabase/ModelDatabase.h | 2 +- .../ModelDatabase/ModelDatabaseRetrieval.cpp | 108 +-------------- .../ModelDatabase/ModelDatabaseRetrieval.h | 24 +--- quasimodo/quasimodo_models/CMakeLists.txt | 4 +- .../src/modelupdater/ModelUpdater.cpp | 126 ++++++++++++------ 6 files changed, 90 insertions(+), 176 deletions(-) diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 0352da6f..4fd9f819 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -95,7 +95,7 @@ target_link_libraries(quasimodo_brain_util ${PCL_LIBRARIES} ${catkin_LIBRARIES}) -add_library(quasimodo_ModelDatabase src/ModelDatabase/ModelDatabase.cpp src/ModelDatabase/ModelDatabaseBasic.cpp src/ModelDatabase/ModelDatabaseRGBHistogram.cpp) +add_library(quasimodo_ModelDatabase src/ModelDatabase/ModelDatabase.cpp src/ModelDatabase/ModelDatabaseBasic.cpp src/ModelDatabase/ModelDatabaseRGBHistogram.cpp src/ModelDatabase/ModelDatabaseRetrieval.cpp ) target_link_libraries(quasimodo_ModelDatabase #${retrieval_libraries} #${benchmark_libraries} diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h index 66621531..ee9aec94 100644 --- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h +++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h @@ -29,5 +29,5 @@ class ModelDatabase{ #include "ModelDatabaseBasic.h" #include "ModelDatabaseRGBHistogram.h" -//#include "ModelDatabaseRetrieval.h" +#include "ModelDatabaseRetrieval.h" #endif // ModelDatabase_H diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp index a8104c34..2dc84764 100644 --- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp +++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp @@ -1,31 +1,12 @@ #include "ModelDatabaseRetrieval.h" -#include -#include -#include - -#include - using namespace std; -using PointT = pcl::PointXYZRGB; -using CloudT = pcl::PointCloud; -using NormalT = pcl::Normal; -using NormalCloudT = pcl::PointCloud; -POINT_CLOUD_REGISTER_POINT_STRUCT (HistT, - (float[250], histogram, histogram) -) -ModelDatabaseRetrieval::ModelDatabaseRetrieval(std::string vpath) : vt_features() +ModelDatabaseRetrieval::ModelDatabaseRetrieval() { - // actually, maybe we should just add some features to the vocabulary so it's not empty, like load an old vocabulary? - // good idea - boost::filesystem::path vocabulary_path(vpath); - dynamic_object_retrieval::load_vocabulary(vt, vocabulary_path); - vt.set_min_match_depth(3); - vt.compute_normalizing_constants(); - training_indices = vt.max_ind(); + } ModelDatabaseRetrieval::~ModelDatabaseRetrieval(){} @@ -56,88 +37,3 @@ std::vector ModelDatabaseRetrieval::search(reglib::Model * mode } return ret; } - - -//Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching -int ModelDatabaseRetrieval::add(pcl::PointCloud::Ptr cloud, double weight) -{ - CloudT::Ptr points(new CloudT); - NormalCloudT::Ptr normals(new NormalCloudT); - for (const pcl::PointXYZRGBNormal& pn : cloud->points) { - points->push_back(PointT()); - points->back().getVector3fMap() = pn.getVector3fMap(); - points->back().getRGBVector4i() = pn.getRGBVector4i(); - normals->push_back(NormalT()); - normals->back().getNormalVector3fMap() = pn.getNormalVector3fMap(); - } - - HistCloudT::Ptr features(new HistCloudT); - CloudT::Ptr keypoints(new CloudT); - dynamic_object_retrieval::compute_features(features, keypoints, points, normals, false, true); - - cout << "Got features!" << endl; - - vt_features.push_back(HistCloudT::Ptr(new HistCloudT(*features))); - - int old_index = vt.max_ind(); - - cout << "Trying to append cloud" << endl; - - vector indices(features->size(), old_index); - - cout << "Features cloud size: " << features->size() << endl; - vt.append_cloud(features, indices, false); - - cout << "Got to append the cloud!" << endl; - - int new_index = vt.max_ind()-1; - - added_indices.insert(make_pair(new_index, added_indices.size())); - - cout << "Added cloud with index: " << new_index << endl; - - return added_indices.size()-1; -} - -// return true if successfull -// return false if fail -bool ModelDatabaseRetrieval::remove(int index) -{ - // don't do this for now - removed_indices.insert(index); - - return false; -} - -//Find the number_of_matches closest matches in dabase to the pointcloud for index -std::vector ModelDatabaseRetrieval::search(int index, int number_of_matches) -{ - using result_type = vocabulary_tree::result_type; - vector scores; - - cout << "Trying to search with index: " << index << endl; - cout << "Number clouds added: " << vt_features.size() << endl; - - vt.query_vocabulary(scores, vt_features[index], 0); - vector rtn; - - cout << "Finished querying, got " << scores.size() << " results!" << endl; - - for (const result_type& r : scores) { - if (rtn.size() >= number_of_matches) { - break; - } - if (r.index < training_indices) { - continue; - } - cout << "Accessing added cloud at vt index: " << r.index << endl; - int return_index = added_indices[r.index]; - if (removed_indices.count(return_index) == 0) { - rtn.push_back(return_index); - } - } - - cout << "Filtered out only the added cloud, got " << rtn.size() << " clouds!" << endl; - - return rtn; -} diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.h b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.h index f19286c7..7c89b63c 100644 --- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.h +++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.h @@ -2,21 +2,8 @@ #define MODELDATABASERETRIEVAL_H #include "ModelDatabase.h" -#include -#include - -using HistT = pcl::Histogram<250>; -using HistCloudT = pcl::PointCloud; class ModelDatabaseRetrieval: public ModelDatabase{ -private: - - vocabulary_tree vt; - vector vt_features; - map added_indices; - int training_indices; - set removed_indices; - public: @@ -24,16 +11,7 @@ class ModelDatabaseRetrieval: public ModelDatabase{ virtual bool remove(reglib::Model * model); virtual std::vector search(reglib::Model * model, int number_of_matches); - virtual int add(pcl::PointCloud::Ptr cloud, double weight = 1); - - // return true if successfull - // return false if fail - virtual bool remove(int index); - - //Find the number_of_matches closest matches in dabase to the pointcloud for index - virtual std::vector search(int index, int number_of_matches); - - ModelDatabaseRetrieval(std::string vpath = "/media/johane/SSDstorage/vocabulary_johan/"); + ModelDatabaseRetrieval(); ~ModelDatabaseRetrieval(); }; diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index b764412f..78afc7b6 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -2,8 +2,8 @@ cmake_minimum_required(VERSION 2.8.3) project(quasimodo_models) #find_package(catkin REQUIRED)# quasimodo_msgs)# COMPONENTS quasimodo_msgs) -set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") - +#set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") +set(CMAKE_CXX_FLAGS "-O4 -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") find_package(catkin REQUIRED COMPONENTS #metaroom_xml_parser diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 56657807..e4223857 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1804,7 +1804,7 @@ void ModelUpdater::getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, d } } -std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ +std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ cv::Mat src = frame->rgb.clone(); unsigned short * depthdata = (unsigned short *)(frame->depth.data); const float idepth = frame->camera->idepth_scale; @@ -1815,11 +1815,11 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int unsigned int width = src.cols; unsigned int height = src.rows; - std::vector dxc; + std::vector dxc; dxc.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0;} - std::vector dyc; + std::vector dyc; dyc.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dyc[i] = 0;} @@ -2108,7 +2108,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int } - std::vector< std::vector > probs2; + std::vector< std::vector > probs2; probs2.push_back(dxc); probs2.push_back(dyc); @@ -2391,21 +2391,21 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector prior; - std::vector< std::vector > connectionId; - std::vector< std::vector > connectionStrength; +// std::vector prior; +// std::vector< std::vector > connectionId; +// std::vector< std::vector > connectionStrength; std::vector< std::vector > interframe_connectionId; std::vector< std::vector > interframe_connectionStrength; - prior.resize(tot_nr_pixels); - connectionId.resize(tot_nr_pixels); - connectionStrength.resize(tot_nr_pixels); +// prior.resize(tot_nr_pixels); +// connectionId.resize(tot_nr_pixels); +// connectionStrength.resize(tot_nr_pixels); interframe_connectionId.resize(tot_nr_pixels); interframe_connectionStrength.resize(tot_nr_pixels); int current_point = 0; - double * priors = new double[3*tot_nr_pixels]; + float * priors = new float[3*tot_nr_pixels]; // double * priors_weight = new double[3*tot_nr_pixels]; // double * estimates = new double[3*tot_nr_pixels]; // double * points = new double[3*tot_nr_pixels]; @@ -2414,14 +2414,14 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector > > pixel_weights; + std::vector< std::vector< std::vector > > pixel_weights; pcl::PointCloud::Ptr cloud (new pcl::PointCloud); for(unsigned int i = 0; i < frames.size(); i++){ int offset = offsets[i]; RGBDFrame * frame = frames[i]; - std::vector< std::vector > probs = getImageProbs(frame,9); + std::vector< std::vector > probs = getImageProbs(frame,9); pixel_weights.push_back(probs); unsigned short * depthdata = (unsigned short *)(frame->depth.data); unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); @@ -2552,13 +2552,13 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector add_tweights( i, weightFG, weightBG ); } - double maxprob_same = 0.999999; + float maxprob_same = 0.99999999999; for(unsigned int i = 0; i < frames.size(); i++){ int offset = offsets[i]; Camera * camera = frames[i]->camera; const unsigned int width = camera->width; const unsigned int height = camera->height; - std::vector< std::vector > & probs = pixel_weights[i]; + std::vector< std::vector > & probs = pixel_weights[i]; for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; @@ -2590,7 +2590,7 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector maxflow(); int dynamic_label = 1; - std::vector labels; + std::vector labels; std::vector dyn_ind; std::vector stat_ind; labels.resize(current_point); @@ -2619,7 +2619,7 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vectorcamera; const unsigned int width = camera->width; const unsigned int height = camera->height; - std::vector< std::vector > & probs = pixel_weights[i]; + std::vector< std::vector > & probs = pixel_weights[i]; for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; @@ -2652,10 +2652,10 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector * dynamic_g = new gc::Graph(nr_dynamic,dynamic_frameConnections+dynamic_interframeConnections); - +printf("LINE: %i\n",__LINE__); for(unsigned long i = 0; i < dyn_ind.size();i++){ dynamic_g -> add_node(); double p_fg = priors[3*dyn_ind[i]+0]; @@ -2672,13 +2672,13 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector add_tweights( i, weightFG, weightBG ); } - +printf("LINE: %i\n",__LINE__); for(unsigned long i = 0; i < frames.size(); i++){ int offset = offsets[i]; Camera * camera = frames[i]->camera; const unsigned int width = camera->width; const unsigned int height = camera->height; - std::vector< std::vector > & probs = pixel_weights[i]; + std::vector< std::vector > & probs = pixel_weights[i]; for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; @@ -2708,7 +2708,7 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcp, vector maxflow(); @@ -2738,25 +2740,63 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector staticdata; - std::vector dynamicdata; - std::vector movingdata; +// std::vector staticdata; +// std::vector dynamicdata; +// std::vector movingdata; +// pcl::PointCloud staticcloud; +// pcl::PointCloud dynamiccloud; +// pcl::PointCloud movingcloud; +//printf("LINE: %i\n",__LINE__); - pcl::PointCloud::Ptr staticcloud (new pcl::PointCloud); - for(unsigned int i = 0; i < current_point; i++){ - if(labels[i] == 0){ - staticdata.push_back(i) - } +//{ - if(labels[i] == 1){ +//} - } +// // pcl::PointCloud::Ptr dynamiccloud (new pcl::PointCloud); +// // pcl::PointCloud::Ptr movingcloud (new pcl::PointCloud); +// for(unsigned int i = 0; i < current_point; i++){ +// pcl::PointXYZRGBNormal p = cloud->points[i]; +// if(labels[i] == 0){ +// staticdata.push_back(i); +// //staticcloud.points.push_back(cloud->points[i]); +// } + +// if(labels[i] == 1){ +// dynamicdata.push_back(i); +// //dynamiccloud.points.push_back(cloud->points[i]); +// } + +// if(labels[i] == 2){ +// movingdata.push_back(i); +// //movingcloud.points.push_back(cloud->points[i]); +// } +// } + +// pcl::search::KdTree::Ptr dynamictree (new pcl::search::KdTree); +// dynamictree->setInputCloud (dynamiccloud); + +// std::vector dynamic_indices; +// pcl::EuclideanClusterExtraction dynamic_ec; +// dynamic_ec.setClusterTolerance (0.02); // 2cm +// dynamic_ec.setMinClusterSize (500); +// dynamic_ec.setMaxClusterSize (250000000); +// dynamic_ec.setSearchMethod (dynamictree); +// dynamic_ec.setInputCloud (dynamiccloud); +// dynamic_ec.extract (dynamic_indices); + +// for (unsigned int d = 0; d < dynamic_indices.size(); d++){ +// std::vector< std::vector > inds; +// inds.resize(mod_fr.size()); + +// pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); +// for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ +// int pid = dynamic_indices[d].indices[ind]; +// inds[dynamic_frameid[pid]].push_back(dynamic_pixelid[pid]); +// cloud_cluster->points.push_back(dynamiccloud->points[pid]); +// } - if(labels[i] == 2){ - } - } // viewer->removeAllPointClouds(); // viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); // viewer->spin(); @@ -2841,7 +2881,7 @@ delete[] priors; // delete[] colors; // delete[] normals; -exit(0); + //return labels; } @@ -2852,7 +2892,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector ModelUpdater::computeDynamicObject(vector bgcp, vector > probs = getImageProbs(frames[i],9); + std::vector< std::vector > probs = getImageProbs(frames[i],9); std::vector frame_prior; std::vector< std::vector > frame_connectionId; @@ -3334,7 +3374,7 @@ std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eig Eigen::Matrix4d p = cp[i].inverse() * cp[j]; getDynamicWeights(false,p.inverse(), cf[i], overlaps, occlusions, cf[j],oldmasks[j],0,0,interframe_connectionId_tmp,interframe_connectionStrength_tmp); } - std::vector< std::vector > probs = getImageProbs(cf[i],5); + std::vector< std::vector > probs = getImageProbs(cf[i],5); printf("starting partition\n"); double start_part = getTime(); @@ -3362,7 +3402,7 @@ std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eig g -> add_tweights( j, weightFG, weightBG ); } - double maxprob_same = 0.999999999; + float maxprob_same = 0.999999999; for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; @@ -3374,7 +3414,7 @@ std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eig ax *= pr; bx *= 1.0-pr; } - double px = ax/(ax+bx); + float px = ax/(ax+bx); double ay = 0.5; double by = 0.5; @@ -3383,7 +3423,7 @@ std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eig ay *= pr; by *= 1.0-pr; } - double py = ay/(ay+by); + float py = ay/(ay+by); if(w > 0){ int other = ind-1; From 413043be72ffbe6755f9f2198d20cd8dae680cc6 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 30 Aug 2016 23:35:02 +0200 Subject: [PATCH 080/255] segmentation... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 4 +- .../metaroom_additional_view_processing.cpp | 10 +- quasimodo/quasimodo_models/CMakeLists.txt | 4 +- .../include/modelupdater/ModelUpdater.h | 16 +- .../weightfunctions/DistanceWeightFunction2.h | 4 + .../DistanceWeightFunction2PPR2.h | 6 + .../quasimodo_models/src/core/RGBDFrame.cpp | 8 +- .../src/modelupdater/ModelUpdater.cpp | 413 +++++++++++++----- .../DistanceWeightFunction2.cpp | 16 + .../DistanceWeightFunction2PPR2.cpp | 205 ++++++++- 10 files changed, 548 insertions(+), 138 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 34887594..acbe26f2 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -317,7 +317,9 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec masks.push_back(mask); } - mu->computeMovingDynamicStatic(bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions + std::vector movemask; + std::vector dynmask; + mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions // mu->computeMovingDynamicStatic(bgcp,bgcf,mod_po_vec[j],mod_fr_vec[j],debugg);//Determine self occlusions std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,debugg);//Determine self occlusions diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index f3c19a8f..93a2d41e 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1032,8 +1032,16 @@ void sendCallback(const std_msgs::String::ConstPtr& msg){ sendMetaroomToServer(msg->data); } -int main(int argc, char** argv){ +//double normalCFD(double value) +//{ +// return 0.5 * erfc(-value * M_SQRT1_2); +//} +int main(int argc, char** argv){ +//for(double v = -4; v <= 4; v+= 0.01){ +// if(v >= 0){printf(" ");} +// printf("%6.6f : %6.6f\n",v,normalCFD(v)); +//}exit(0); const rlim_t kStackSize = 256 * 1024 * 1024; // min stack size = 256 MB struct rlimit rl; unsigned long result; diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 78afc7b6..b55034c5 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -3,7 +3,7 @@ project(quasimodo_models) #find_package(catkin REQUIRED)# quasimodo_msgs)# COMPONENTS quasimodo_msgs) #set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") -set(CMAKE_CXX_FLAGS "-O4 -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") +set(CMAKE_CXX_FLAGS "-O2 -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") find_package(catkin REQUIRED COMPONENTS #metaroom_xml_parser @@ -53,7 +53,7 @@ FIND_PACKAGE(Ceres REQUIRED) INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) add_library(quasimodo_weightlib src/weightfunctions/DistanceWeightFunction2.cpp src/weightfunctions/DistanceWeightFunction2PPR.cpp src/weightfunctions/DistanceWeightFunction2PPR2.cpp) -target_link_libraries(quasimodo_weightlib ceres glog ${catkin_LIBRARIES}) +target_link_libraries(quasimodo_weightlib ceres glog ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) add_library(quasimodo_core src/core/Util.cpp src/core/Camera.cpp src/core/RGBDFrame.cpp) target_link_libraries(quasimodo_core quasimodo_weightlib ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index d3030d3f..78eaf117 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -22,6 +22,18 @@ #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + //#include "../../densecrf/include/densecrf.h" namespace reglib @@ -92,12 +104,12 @@ namespace reglib virtual void getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2); virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); - virtual void getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); + virtual void getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength,double debugg = false); virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks); virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg = false); - virtual void computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); + virtual void computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); virtual OcclusionScore computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1, bool debugg = false); virtual OcclusionScore computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp = Matrix4d::Identity(), int step = 1, bool debugg = false); diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h index d14540ac..8a448299 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h @@ -44,7 +44,11 @@ class DistanceWeightFunction2 DistanceWeightFunction2(); ~DistanceWeightFunction2(); + MatrixXd getMat(std::vector & vec); + + virtual void computeModel(std::vector & vec); virtual void computeModel(MatrixXd mat); + virtual VectorXd getProbs(std::vector & vec); virtual VectorXd getProbs(MatrixXd mat); virtual double getProb(double d); virtual double getNoise(); diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h index 61ea57bf..4d171df3 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h @@ -11,6 +11,12 @@ #include "gflags/gflags.h" #include "glog/logging.h" +#include +#include +#include +#include +#include + using namespace Eigen; namespace reglib{ diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index a584ff7f..d916442d 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -121,7 +121,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int func->maxp = 0.99; func->startreg = 0.0; func->debugg_print = false; - func->bidir = true; + //func->bidir = true; func->maxd = 256.0; func->histogram_size = 256; func->fixed_histogram_size = true; @@ -198,7 +198,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int funcZ->zeromean = true; funcZ->startreg = 0.002; funcZ->debugg_print = false; - funcZ->bidir = true; + funcZ->bidir = false; funcZ->maxp = 0.999999; funcZ->maxd = 0.1; funcZ->histogram_size = 100; @@ -416,7 +416,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt funcZ->zeromean = true; funcZ->startreg = 0.002; funcZ->debugg_print = false; - funcZ->bidir = true; + funcZ->bidir = false; funcZ->maxp = 0.999999; funcZ->maxd = 0.1; funcZ->histogram_size = 100; @@ -454,7 +454,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt - int dilation_size = 3; + int dilation_size = 4; cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) ); /* cv::Mat det2; diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index e4223857..d898447e 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1902,7 +1902,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int b func->maxp = 0.9999; func->startreg = 0.5; func->debugg_print = false; - func->bidir = true; + //func->bidir = true; func->maxd = 256.0; func->histogram_size = 256; func->fixed_histogram_size = true; @@ -1977,7 +1977,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int b funcZ->zeromean = true; funcZ->startreg = 0.002; funcZ->debugg_print = false; - funcZ->bidir = true; + //funcZ->bidir = true; funcZ->maxp = 0.999999; funcZ->maxd = 0.1; funcZ->histogram_size = 100; @@ -2098,7 +2098,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int b float py = ay/(ay+by); float current = 0; if(!frame->det_dilate.data[ind]){ current = (1-py) * float(maxima_dydata[ind]);} - else{ current = std::max(float(1-probs[probs.size()-1][ind]),0.99f*(1-py) * float(maxima_dydata[ind]));} + else{ current = std::max(float(1-probs[probs.size()-1][ind]),0.9f*(1-py) * float(maxima_dydata[ind]));} probY = 1-current; } @@ -2174,7 +2174,12 @@ std::vector doInference(std::vector & prior, std::vector< std::vect return labels; } -void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ +double normalCFD(double value) +{ + return 0.5 * erfc(-value * M_SQRT1_2); +} + +void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); @@ -2307,7 +2312,7 @@ void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * ove unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; + float dst_nx = dst_normalsdata[3*dst_ind+0]; if(dst_z > 0 && dst_nx != 2){ float dst_ny = dst_normalsdata[3*dst_ind+1]; float dst_nz = dst_normalsdata[3*dst_ind+2]; @@ -2324,47 +2329,54 @@ void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * ove double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / sqrt(dst_noise*dst_noise + point_noise*point_noise); - if(offset1 >= 0 && offset2 >= 0){ - if(fabs(d) < 0.02){ - interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); - interframe_connectionStrength[offset1+src_ind].push_back(2); - } - } +// if(offset1 >= 0 && offset2 >= 0){ +// if(fabs(d) < 0.02){ +// interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); +// interframe_connectionStrength[offset1+src_ind].push_back(2); +// } +// } double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; if(dst_detdata[dst_ind] != 0){continue;} if(src_detdata[src_ind] != 0){continue;} - if(fabs(d) < 0.02){//If close, according noises, and angle of the surfaces similar: FUSE - if(surface_angle > 0.9){ - - - - - overlaps[src_ind] = std::max(overlaps[src_ind],0.9); - if(offset1 >= 0 && offset2 >= 0){ - interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); - interframe_connectionStrength[offset1+src_ind].push_back(-log(1-overlaps[src_ind])); - } - // overlaps[src_ind]++; - // if(debugg){ - //// dst_cloud->points[dst_ind].r = 0; - //// dst_cloud->points[dst_ind].g = 255; - //// dst_cloud->points[dst_ind].b = 0; - // src_cloud->points[src_ind].r = 0; - // src_cloud->points[src_ind].g = 255; - // src_cloud->points[src_ind].b = 0; - // } - } - }else if(tz < dst_z){ - occlusions[src_ind] = std::max(occlusions[src_ind],0.9); - if(debugg){ - src_cloud->points[src_ind].r = 255; - src_cloud->points[src_ind].g = 0; - src_cloud->points[src_ind].b = 255; + d = fabs(d); + if(tz > dst_z){d = -d;} + if(store_distance){ + dvec.push_back(d); + nvec.push_back(1-surface_angle); + }else{ + double noise = 0.02; + double p_overlap = exp(-0.5*(d*d)/(noise*noise)); + p_overlap = std::min(0.9,p_overlap); + double p_occlusion = (1.0-p_overlap)*normalCFD(d/noise); + double noise_angle = 0.05; + double p_overlap_angle = exp(-0.5*((1-surface_angle)*(1-surface_angle))/(noise_angle*noise_angle)); + p_overlap *= p_overlap_angle; + overlaps[src_ind] = std::max(overlaps[src_ind],p_overlap); + occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); + if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0){ + interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); + interframe_connectionStrength[offset1+src_ind].push_back(-log(1-overlaps[src_ind])); } } +// if(fabs(d) < 0.02){//If close, according noises, and angle of the surfaces similar: FUSE +// if(surface_angle > 0.9){ +// overlaps[src_ind] = std::max(overlaps[src_ind],0.9); +// if(offset1 >= 0 && offset2 >= 0){ +// interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); +// interframe_connectionStrength[offset1+src_ind].push_back(-log(1-overlaps[src_ind])); +// } +// } +// }else if(tz < dst_z){ +// occlusions[src_ind] = std::max(occlusions[src_ind],0.9); +// if(debugg){ +// src_cloud->points[src_ind].r = 255; +// src_cloud->points[src_ind].g = 0; +// src_cloud->points[src_ind].b = 255; +// } +// } } } } @@ -2379,7 +2391,8 @@ void ModelUpdater::getDynamicWeights(Matrix4d p, RGBDFrame* frame1, double * ove } } -void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ +void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ + int tot_nr_pixels = 0; std::vector offsets; @@ -2391,27 +2404,135 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector prior; -// std::vector< std::vector > connectionId; -// std::vector< std::vector > connectionStrength; std::vector< std::vector > interframe_connectionId; std::vector< std::vector > interframe_connectionStrength; - -// prior.resize(tot_nr_pixels); -// connectionId.resize(tot_nr_pixels); -// connectionStrength.resize(tot_nr_pixels); interframe_connectionId.resize(tot_nr_pixels); interframe_connectionStrength.resize(tot_nr_pixels); int current_point = 0; float * priors = new float[3*tot_nr_pixels]; - // double * priors_weight = new double[3*tot_nr_pixels]; - // double * estimates = new double[3*tot_nr_pixels]; - // double * points = new double[3*tot_nr_pixels]; - // double * noise = new double[3*tot_nr_pixels]; - // double * colors = new double[3*tot_nr_pixels]; - // double * normals = new double[3*tot_nr_pixels]; + + //bool store_distance = true; + + std::vector dvec; + std::vector nvec; + DistanceWeightFunction2 * dfunc; + DistanceWeightFunction2 * nfunc; + +// for(unsigned int i = 0; i < frames.size(); i++){ +// for(unsigned int j = 0; j < frames.size(); j++){ +// if(i == j){continue;} +// Eigen::Matrix4d p = poses[i].inverse() * poses[j]; +// getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i], 0, 0, frames[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); +// } + +// for(unsigned int j = 0; j < bgcf.size(); j++){ +// Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; +// getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i], 0, 0, bgcf[j],-1,-1,interframe_connectionId,interframe_connectionStrength,false); +// } +// } + +// double dstdval = 0; +// for(unsigned int i = 0; i < dvec.size(); i++){ +// dstdval += dvec[i]*dvec[i]; +// } +// dstdval = sqrt(dstdval/double(dvec.size()-1)); + + +// DistanceWeightFunction2PPR2 * dfuncTMP = new DistanceWeightFunction2PPR2(); +// dfunc = dfuncTMP; +// dfuncTMP->refine_mean = true; +// dfuncTMP->zeromean = false; +// dfuncTMP->startreg = 0.0001; +// dfuncTMP->debugg_print = true; +// dfuncTMP->bidir = true; +// dfuncTMP->maxp = 0.999999; +// dfuncTMP->maxd = 0.03; +// dfuncTMP->histogram_size = 1000; +// dfuncTMP->fixed_histogram_size = true; +// dfuncTMP->startmaxd = dfuncTMP->maxd; +// dfuncTMP->starthistogram_size = dfuncTMP->histogram_size; +// dfuncTMP->blurval = 0.5; +// dfuncTMP->stdval2 = dstdval; +// dfuncTMP->maxnoise = dstdval; +// dfuncTMP->reset(); +// dfunc->computeModel(dvec); + + +// DistanceWeightFunction2PPR2 * nfuncTMP = new DistanceWeightFunction2PPR2(); +// nfunc = nfuncTMP; +// nfuncTMP->startreg = 0.0001; +// nfuncTMP->debugg_print = true; +// nfuncTMP->bidir = true; +// nfuncTMP->zeromean = false; +// nfuncTMP->maxp = 0.999999; +// nfuncTMP->maxd = 1.0; +// nfuncTMP->histogram_size = 1000; +// nfuncTMP->fixed_histogram_size = true; +// nfuncTMP->startmaxd = nfuncTMP->maxd; +// nfuncTMP->starthistogram_size = nfuncTMP->histogram_size; +// nfuncTMP->blurval = 0.5; +// nfuncTMP->stdval2 = 1; +// nfuncTMP->maxnoise = 1; +// nfuncTMP->reset(); +// nfunc->computeModel(nvec); + +//// for(unsigned int w = 1; w < width-1;w++){ +//// for(unsigned int h = 1; h < height-1;h++){ +//// int ind = h*width+w; +//// float z = idepth*float(depthdata[ind]); + +//// if(w > 1){ +//// int dir = -1; +//// int other2 = ind+2*dir; +//// int other = ind+dir; + +//// float z3 = idepth*float(depthdata[other2]); +//// float z2 = idepth*float(depthdata[other]); +//// z2 = 2*z2-z3; + +//// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} +//// } + +//// if(h > 1){ +//// int dir = -width; +//// int other2 = ind+2*dir; +//// int other = ind+dir; + +//// float z3 = idepth*float(depthdata[other2]); +//// float z2 = idepth*float(depthdata[other]); +//// z2 = 2*z2-z3; + +//// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} +//// } +//// } +//// } + +//// std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} +//// std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + +//// for(unsigned int w = 1; w < width-1;w++){ +//// for(unsigned int h = 1; h < height-1;h++){ +//// int ind = h*width+w; +//// float z = idepth*float(depthdata[ind]); + +//// if(w > 1){ +//// int dir = -1; +//// int other2 = ind+2*dir; +//// int other = ind+dir; + + +//// float z3 = idepth*float(depthdata[other2]); +//// float z2 = idepth*float(depthdata[other]); + +//// float dz = fabs(z-z2); +//// if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} +//// if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} +//delete dfuncTMP; + +//exit(0); + int frameConnections = 0; std::vector< std::vector< std::vector > > pixel_weights; @@ -2424,8 +2545,6 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector > probs = getImageProbs(frame,9); pixel_weights.push_back(probs); unsigned short * depthdata = (unsigned short *)(frame->depth.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - float * normalsdata = (float *)(frame->normals.data); Camera * camera = frame->camera; const unsigned int width = camera->width; @@ -2448,7 +2567,7 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcp, vector bgcp, vector bgcp, vector add_tweights( i, weightFG, weightBG ); } - float maxprob_same = 0.99999999999; + double maxprob_same = 0.99999999999; for(unsigned int i = 0; i < frames.size(); i++){ int offset = offsets[i]; Camera * camera = frames[i]->camera; @@ -2564,7 +2683,7 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector 0 && probs[0][ind] > 0.00000001){ int other = ind-1; - double p_same = std::min(probs[0][ind],maxprob_same); + double p_same = std::min(double(probs[0][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); g -> add_edge( ind+offset, other+offset, weight, weight ); @@ -2572,7 +2691,7 @@ void ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector 0 && probs[1][ind] > 0.00000001){ int other = ind-width; - double p_same = std::min(probs[1][ind],maxprob_same); + double p_same = std::min(double(probs[1][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); g -> add_edge( ind+offset, other+offset, weight, weight ); @@ -2687,7 +2806,7 @@ printf("LINE: %i\n",__LINE__); int other = ind-1; if(labels[ind+offset] == labels[other+offset]){ //dynamic_frameConnections++; - double p_same = std::min(probs[0][ind],maxprob_same); + double p_same = std::min(double(probs[0][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight ); @@ -2698,7 +2817,7 @@ printf("LINE: %i\n",__LINE__); int other = ind-width; if(labels[ind+offset] == labels[other+offset]){ //dynamic_frameConnections++; - double p_same = std::min(probs[0][ind],maxprob_same); + double p_same = std::min(double(probs[0][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight ); @@ -2739,70 +2858,127 @@ printf("LINE: %i\n",__LINE__); } delete dynamic_g; + std::vector staticdata; + std::vector dynamicdata; + std::vector movingdata; + pcl::PointCloud::Ptr staticcloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dynamiccloud (new pcl::PointCloud); + pcl::PointCloud::Ptr movingcloud (new pcl::PointCloud); + for(unsigned int i = 0; i < current_point; i++){ + //pcl::PointXYZRGBNormal p = cloud->points[i]; + if(labels[i] == 0){ + staticdata.push_back(i); + staticcloud->points.push_back(cloud->points[i]); + } + if(labels[i] == 1){ + dynamicdata.push_back(i); + dynamiccloud->points.push_back(cloud->points[i]); + } -// std::vector staticdata; -// std::vector dynamicdata; -// std::vector movingdata; -// pcl::PointCloud staticcloud; -// pcl::PointCloud dynamiccloud; -// pcl::PointCloud movingcloud; -//printf("LINE: %i\n",__LINE__); - -//{ - -//} - -// // pcl::PointCloud::Ptr dynamiccloud (new pcl::PointCloud); -// // pcl::PointCloud::Ptr movingcloud (new pcl::PointCloud); -// for(unsigned int i = 0; i < current_point; i++){ -// pcl::PointXYZRGBNormal p = cloud->points[i]; -// if(labels[i] == 0){ -// staticdata.push_back(i); -// //staticcloud.points.push_back(cloud->points[i]); -// } + if(labels[i] == 2){ + movingdata.push_back(i); + movingcloud->points.push_back(cloud->points[i]); + } + } -// if(labels[i] == 1){ -// dynamicdata.push_back(i); -// //dynamiccloud.points.push_back(cloud->points[i]); -// } + pcl::search::KdTree::Ptr dynamictree (new pcl::search::KdTree); + dynamictree->setInputCloud (dynamiccloud); + + std::vector dynamic_indices; + pcl::EuclideanClusterExtraction dynamic_ec; + dynamic_ec.setClusterTolerance (0.03); // 2cm + dynamic_ec.setMinClusterSize (1); + dynamic_ec.setMaxClusterSize (250000000); + dynamic_ec.setSearchMethod (dynamictree); + dynamic_ec.setInputCloud (dynamiccloud); + dynamic_ec.extract (dynamic_indices); +printf("dynamic\n"); + for (unsigned int d = 0; d < dynamic_indices.size(); d++){ +// std::vector< std::vector > inds; +// inds.resize(mod_fr.size()); -// if(labels[i] == 2){ -// movingdata.push_back(i); -// //movingcloud.points.push_back(cloud->points[i]); -// } -// } + double score = 0; + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ + int pid = dynamic_indices[d].indices[ind]; + int dind = dynamicdata[pid]; + score += priors[3*dind+1] -0.5*(priors[3*dind+0]+priors[3*dind+2]); + cloud_cluster->points.push_back(dynamiccloud->points[pid]); + } -// pcl::search::KdTree::Ptr dynamictree (new pcl::search::KdTree); -// dynamictree->setInputCloud (dynamiccloud); + printf("score: %f\n",score); + if(score < 100){ + for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ + labels[dynamicdata[dynamic_indices[d].indices[ind]]] = 0; + } + }else{ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); + viewer->spin(); + } + } -// std::vector dynamic_indices; -// pcl::EuclideanClusterExtraction dynamic_ec; -// dynamic_ec.setClusterTolerance (0.02); // 2cm -// dynamic_ec.setMinClusterSize (500); -// dynamic_ec.setMaxClusterSize (250000000); -// dynamic_ec.setSearchMethod (dynamictree); -// dynamic_ec.setInputCloud (dynamiccloud); -// dynamic_ec.extract (dynamic_indices); + pcl::search::KdTree::Ptr movingtree (new pcl::search::KdTree); + movingtree->setInputCloud (movingcloud); + + std::vector moving_indices; + pcl::EuclideanClusterExtraction moving_ec; + moving_ec.setClusterTolerance (0.03); // 2cm + moving_ec.setMinClusterSize (1); + moving_ec.setMaxClusterSize (250000000); + moving_ec.setSearchMethod (movingtree); + moving_ec.setInputCloud (movingcloud); + moving_ec.extract (moving_indices); + + printf("moving\n"); + for (unsigned int d = 0; d < moving_indices.size(); d++){ + double score = 0; + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ + int pid = moving_indices[d].indices[ind]; + int dind = movingdata[pid]; + score += priors[3*dind+2] -0.5*(priors[3*dind+0]+priors[3*dind+1]); + cloud_cluster->points.push_back(movingcloud->points[pid]); + } -// for (unsigned int d = 0; d < dynamic_indices.size(); d++){ -// std::vector< std::vector > inds; -// inds.resize(mod_fr.size()); + printf("score: %f\n",score); + if(score < 100){ + for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ + labels[movingdata[moving_indices[d].indices[ind]]] = 0; + } + }else{ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); + viewer->spin(); + } + } -// pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); -// for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ -// int pid = dynamic_indices[d].indices[ind]; -// inds[dynamic_frameid[pid]].push_back(dynamic_pixelid[pid]); -// cloud_cluster->points.push_back(dynamiccloud->points[pid]); -// } +// printf("nr_moving: %i\n",nr_moving); -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); -// viewer->spin(); + unsigned long current = 0; + for(unsigned long i = 0; i < frames.size(); i++){ + Camera * camera = frames[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; -// printf("nr_moving: %i\n",nr_moving); + cv::Mat m; + m.create(height,width,CV_8UC1); + unsigned char * mdata = (unsigned char*)m.data; + + cv::Mat d; + d.create(height,width,CV_8UC1); + unsigned char * ddata = (unsigned char*)d.data; + for(int j = 0; j < width*height; j++){ + mdata[j] = labels[current]==2; + ddata[j] = labels[current]==1; + current++; + } + movemask.push_back(m); + dynmask.push_back(d); + } if(debugg){ @@ -2849,6 +3025,7 @@ if(debugg){ viewer->removeAllPointClouds(); viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); viewer->spin(); + } @@ -2882,6 +3059,8 @@ delete[] priors; // delete[] normals; //return labels; + printf("done\n"); + exit(0); } @@ -2892,7 +3071,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector ModelUpdater::computeDynamicObject(vector bgcp, vector 0 && w < width-1){ int other = ind-1; - double p_same = std::min(probs[0][ind],maxprob_same); + double p_same = std::min(double(probs[0][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); if(!std::isnan(weight) && weight > 0){ @@ -3016,7 +3195,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector 0 && h < height-1){ int other = ind-width; - double p_same = std::min(probs[1][ind],maxprob_same); + double p_same = std::min(double(probs[1][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp index aa5a9a3c..5e9b47e6 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp @@ -10,6 +10,22 @@ DistanceWeightFunction2::DistanceWeightFunction2(){ } DistanceWeightFunction2::~DistanceWeightFunction2(){} +MatrixXd DistanceWeightFunction2::getMat(std::vector & vec){ + unsigned long nr_data = vec.size(); + Eigen::VectorXd v (nr_data); + for(unsigned long i = 0; i < nr_data; i++){v(i) = vec[i];} + return v; +} + +void DistanceWeightFunction2::computeModel(std::vector & vec){ + computeModel(getMat(vec)); +} + + +VectorXd DistanceWeightFunction2::getProbs(std::vector & vec){ + return getProbs(getMat(vec)); +} + void tukey_weight(Eigen::VectorXd& r, double p) { for(int i=0; i p) r(i) = 0.0; diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp index ebbb40de..49defbcc 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp @@ -362,6 +362,71 @@ const double step_h = 0.00001; const unsigned int step_iter = 25; const double cutoff_exp = -14; +double scoreCurrent3(double mul, double mean, double stddiv, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + double sum = 0; + double invstd = 1.0/stddiv; + for(unsigned int i = 0; i < nr_data; i++){ + double dx = fabs(X[i] - mean)*invstd; + double inp = -0.5*pow(dx,power); + if(inp < cutoff_exp){sum += Y[i];} + else{ + double diff = Y[i]*Y[i]*(mul*exp(inp) - Y[i]); + if(diff > 0){ sum += costpen*diff;} + else{ sum -= diff;} + } + } + return sum; +} + +double fitStdval3(double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + int iter = 25; + double h = 0.000000001; + + double std_max = std_mid*2; + double std_min = 0; + for(int i = 0; i < iter; i++){ + std_mid = (std_max+std_min)/2; + double std_neg = scoreCurrent3(mul,mean,std_mid-h,power,X,Y,nr_data,costpen); + double std_pos = scoreCurrent3(mul,mean,std_mid+h,power,X,Y,nr_data,costpen); + if(std_neg < std_pos){ std_max = std_mid;} + else{ std_min = std_mid;} + } + return std_mid; +} + +double fitPower3(double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + int iter = 25; + double h = 0.000000001; + + double power_max = power*2; + double power_min = 0.001; + for(int i = 0; i < iter; i++){ + power = (power_max+power_min)/2; + double std_neg = scoreCurrent3(mul,mean,std_mid,power-h,X,Y,nr_data,costpen); + double std_pos = scoreCurrent3(mul,mean,std_mid,power+h,X,Y,nr_data,costpen); + if(std_neg < std_pos){ power_max = power;} + else{ power_min = power;} + } + power = (power_max+power_min)/2; + return power; +} + +double fitMean3(double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + int iter = 10; + double h = 0.000000001; + double mean_max = mean+10; + double mean_min = mean-10; + + for(int i = 0; i < iter; i++){ + mean = (mean_max+mean_min)/2; + double std_neg = scoreCurrent3(mul,mean-h,std_mid,power,X,Y,nr_data,costpen); + double std_pos = scoreCurrent3(mul,mean+h,std_mid,power,X,Y,nr_data,costpen); + if(std_neg < std_pos){ mean_max = mean;} + else{ mean_min = mean;} + } + return mean; +} + double scoreCurrent2(double bias, double mul, double mean, double stddiv, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ double info = -0.5/(stddiv*stddiv); @@ -413,10 +478,13 @@ double fitBias2(double bias, double mul, double mean, double std_mid, std::vecto } double fitMean2(double bias,double mul, double mean, double std_mid, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ - int iter = 25; + int iter = 10; double h = 0.000000001; - double mean_max = mean*2; - double mean_min = 0; + double mean_max = mean+1; + double mean_min = mean-1; + + // double mean_max = mean*2; + // double mean_min = 0; for(int i = 0; i < iter; i++){ mean = (mean_max+mean_min)/2; @@ -490,9 +558,93 @@ Gaussian3 getModel(double & stdval,std::vector & hist, bool uniform_bias, } return Gaussian3(mul,mean,stdval); } +/* +void getGGModel(std::vector & hist, unsigned int nr_bins){ + + + double mul = hist[0]; + double mean = 0; + //unsigned int nr_bins = hist.size(); + + for(unsigned int k = 1; k < nr_bins; k++){ + if(hist[k] > mul){ + mul = hist[k]; + mean = k; + } + } + + double maxhist = mul; + + + std::vector X; + std::vector Y; + for(unsigned int k = 0; k < nr_bins; k++){ + if(hist[k] > mul*0.01){ + X.push_back(k); + Y.push_back(hist[k]); + } + } + + unsigned int nr_data_opt = X.size(); + + double ysum = 0; + for(unsigned int i = 0; i < nr_data_opt; i++){ysum += fabs(Y[i]);} + + double std_mid = 0; + for(unsigned int i = 0; i < nr_data_opt; i++){std_mid += (X[i]-mean)*(X[i]-mean)*fabs(Y[i])/ysum;} + double stdval = sqrt(std_mid); + + double power = 2; + //printf("mean: %10.10f mul: %10.10f\n",mean,mul,std_mid,power); + + printf("hist = ["); + for(unsigned int k = 0; k < nr_bins; k++){printf("%5.5f ",float(hist[k])/maxhist);} + printf("];\n"); + + char buf [1024*1024]; + sprintf(buf,"ggd = ["); + int nr_refineiters = 10; + + double pre_mean = mean; + double pre_std = stdval; + double pre_power = power; + + for(int i = 0; i < nr_refineiters; i++){ + printf("ggd%i = [",i); + for(unsigned int k = 0; k < nr_bins; k++){ + printf("%5.5f ",mul/maxhist * exp(-0.5*pow(fabs(double(k)-mean)/stdval,power))); + } + printf("];\n"); + + if(i != 0){sprintf(buf,";");} + sprintf(buf,"%10.10f, %10.10f, %10.10f, %10.10f",mul,mean,stdval,power); + + stdval = fitStdval3( mul,mean,stdval,power,X,Y,nr_data_opt,3); + power = fitPower3( mul,mean,stdval,power,X,Y,nr_data_opt,3); + mean = fitMean3( mul,mean,stdval,power,X,Y,nr_data_opt,3); + if(fabs(mean - pre_mean) < 0.1 && fabs(stdval - pre_std) < 0.1 && fabs(power - pre_power) < 0.01){ + break; + } + pre_mean = mean; + pre_std = stdval; + pre_power = power; + } + sprintf(buf,"];\n"); + printf("%s\n",buf); + + printf("mean: %10.10f mul: %10.10f std: %10.10f power: %10.10f\n",mean,mul,std_mid,power); + //return Gaussian3(mul,mean,stdval); +} +*/ double DistanceWeightFunction2PPR2::getNoise(){return regularization+noiseval;}// + stdval*double(histogram_size)/maxd;} +#include +#include +#include +#include +#include + void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ //debugg_print = false; @@ -558,10 +710,7 @@ if(!fixed_histogram_size){ double ind = fabs(mat(k,j))*histogram_mul; double w2 = ind-int(ind); double w1 = 1-w2; - if(ind >= 0 && (ind+0.5) < histogram_size){ - histogram[int(ind+0.5)]++; - } - //if(j % 5000 == 0){printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind);} + if(ind >= 0 && (ind+0.5) < histogram_size){histogram[int(ind+0.5)]++;} } } histogram[0]*=2; @@ -578,12 +727,33 @@ if(!fixed_histogram_size){ } } } + +if(bidir){ + printf("%i\n",__LINE__); + for(unsigned int j = 0; j < histogram_size; j++){histogram[j] = 0;} + for(unsigned int j = 0; j < nr_data; j++){ + for(int k = 0; k < nr_dim; k++){ + double ind = double(histogram_size)*(mat(k,j)+maxd)/(2.0*maxd); + if(ind >= 0 && (ind+0.00001) < histogram_size){ + histogram[int(ind+0.00001)]++; + } + } + } + + //if(debugg_print){ + //} +} start_time = getCurrentTime3(); blurHistogram3(blur_histogram,histogram,blurval,histogram_size,debugg_print); //blurHistogram2(blur_histogram,histogram,blurval,false); +//if(bidir){ +// getGGModel(blur_histogram,histogram_size); +// exit(0); +//} Gaussian3 g = getModel(stdval,blur_histogram,uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); + //printf("found stdval: %f\n",stdval); stdval2 = 0; Gaussian3 g2 = getModel(stdval2,histogram,uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); @@ -662,10 +832,23 @@ if(!fixed_histogram_size){ nr_inliers += d; } - if(debugg_print){printf("hist = ["); for(int k = 0; k < 300 && k < histogram_size; k++){printf("%i ",int(histogram[k]));} printf("];\n");} - if(debugg_print){printf("noise = ["); for(int k = 0; k < 300 && k < int(noise.size()); k++){printf("%i ",int(noise[k]));} printf("];\n");} - if(debugg_print){printf("hist_smooth = ["); for(int k = 0; k < 300 && k < int(blur_histogram.size()); k++){printf("%i ",int(blur_histogram[k]));} printf("];\n");} - if(debugg_print){printf("new_histogram = ["); for(int k = 0; k < 300 && k < int(new_histogram.size()); k++){printf("%i ",int(new_histogram[k]));} printf("];\n");} + + + if(bidir){ + if(debugg_print){printf("mul: %10.10f mean: %10.10f stdval: %10.10f\n",g.mul,g.mean,g.stdval);} + if(debugg_print){printf("hist = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){ printf("%i ",int(histogram[k]));} printf("];\n");} + if(debugg_print){printf("noise = ["); for(int k = 0; k < 3000 && k < int(noise.size()); k++){ printf("%i ",int(noise[k]));} printf("];\n");} + if(debugg_print){printf("hist_smooth = ["); for(int k = 0; k < 3000 && k < int(blur_histogram.size()); k++){ printf("%i ",int(blur_histogram[k]));} printf("];\n");} + if(debugg_print){printf("clf; hold on; plot(hist,'r'); plot(noise,'b'); plot(hist_smooth,'g');\n");} + exit(0); + } + + + if(debugg_print){printf("hist = ["); for(int k = 0; k < 300 && k < histogram_size; k++){ printf("%i ",int(histogram[k]));} printf("];\n");} + if(debugg_print){printf("noise = ["); for(int k = 0; k < 300 && k < int(noise.size()); k++){ printf("%i ",int(noise[k]));} printf("];\n");} + if(debugg_print){printf("hist_smooth = ["); for(int k = 0; k < 300 && k < int(blur_histogram.size()); k++){ printf("%i ",int(blur_histogram[k]));} printf("];\n");} + //if(debugg_print){printf("new_histogram = ["); for(int k = 0; k < 300 && k < int(new_histogram.size()); k++){ printf("%i ",int(new_histogram[k]));} printf("];\n");} + if(debugg_print){printf("clf; hold on; plot(hist,'r'); plot(noise,'b'); plot(hist_smooth,'g');\n");} if(false){printf("meanoffset: %f stdval2: %f stdval: %f regularization: %f\n",meanval2,stdval2,noiseval,regularization);} if(!fixed_histogram_size && update_size ){ double next_maxd = meanval2 + (stdval2 + regularization)*target_length; From 26d824f8dfb5d1bd7f92c7b2c0ae1b6eee23a78e Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 31 Aug 2016 17:12:55 +0200 Subject: [PATCH 081/255] ... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 79 ++--- .../metaroom_additional_view_processing.cpp | 105 ++++--- .../src/modelupdater/ModelUpdater.cpp | 270 +++++++++--------- .../modelupdater/ModelUpdaterBasicFuse.cpp | 4 +- 4 files changed, 244 insertions(+), 214 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index acbe26f2..927e8246 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -320,41 +320,50 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec std::vector movemask; std::vector dynmask; mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions - // mu->computeMovingDynamicStatic(bgcp,bgcf,mod_po_vec[j],mod_fr_vec[j],debugg);//Determine self occlusions - - std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,debugg);//Determine self occlusions - std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,debugg);//Determine external occlusions - std::vector dynamic_masks; - for(unsigned int i = 0; i < model->frames.size(); i++){ - reglib::RGBDFrame * frame = model->frames[i]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} - - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); - unsigned char * externalmaskdata = (unsigned char *)(external_masks[i].data); - for(unsigned int k = 0; k < cam->height * cam->width;k++){ - if(externalmaskdata[k] == 0 && internalmaskdata[k] != 0 ){ - maskdata[k] = 255; - }else{ - maskdata[k] = 0; - } - } - - dynamic_masks.push_back(mask); - - // cv::imshow( "rgb", frame->rgb ); - // cv::imshow( "internal_masks", internal_masks[i] ); - // cv::imshow( "externalmask", external_masks[i] ); - // cv::imshow( "dynamic_mask", dynamic_masks[i] ); - // cv::waitKey(0); - } - - internal.push_back(internal_masks); - external.push_back(external_masks); - dynamic.push_back(dynamic_masks); + external.push_back(movemask); + internal.push_back(masks); + dynamic.push_back(dynmask); + +// for(unsigned int i = 0; i < model->frames.size(); i++){ +// cv::imshow( "rgb", model->frames[i]->rgb ); +// cv::imshow( "movemask", movemask[i] ); +// cv::imshow( "dynmask", dynmask[i] ); +// cv::waitKey(0); +// } + +// std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,debugg);//Determine self occlusions +// std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,debugg);//Determine external occlusions +// std::vector dynamic_masks; +// for(unsigned int i = 0; i < model->frames.size(); i++){ +// reglib::RGBDFrame * frame = model->frames[i]; +// reglib::Camera * cam = frame->camera; +// cv::Mat mask; +// mask.create(cam->height,cam->width,CV_8UC1); +// unsigned char * maskdata = (unsigned char *)(mask.data); +// for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + +// unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); +// unsigned char * externalmaskdata = (unsigned char *)(external_masks[i].data); +// for(unsigned int k = 0; k < cam->height * cam->width;k++){ +// if(externalmaskdata[k] == 0 && internalmaskdata[k] != 0 ){ +// maskdata[k] = 255; +// }else{ +// maskdata[k] = 0; +// } +// } + +// dynamic_masks.push_back(mask); + +// // cv::imshow( "rgb", frame->rgb ); +// // cv::imshow( "internal_masks", internal_masks[i] ); +// // cv::imshow( "externalmask", external_masks[i] ); +// // cv::imshow( "dynamic_mask", dynamic_masks[i] ); +// // cv::waitKey(0); +// } + +// internal.push_back(internal_masks); +// external.push_back(external_masks); +// dynamic.push_back(dynamic_masks); } delete reg; diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 93a2d41e..828a460b 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -644,41 +644,45 @@ void processMetaroom(std::string path){ std::vector< reglib::Model * > models; models.push_back(fullmodel); - +printf("%s::%i\n",__FILE__,__LINE__); std::vector< std::vector< cv::Mat > > internal; std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; - +printf("%s::%i\n",__FILE__,__LINE__); quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization_lvl > 0); - +printf("%s::%i\n",__FILE__,__LINE__); bg->fullDelete(); +printf("%s::%i\n",__FILE__,__LINE__); delete bg; - +printf("%s::%i\n",__FILE__,__LINE__); remove_old_seg(sweep_folder); - +printf("%s::%i\n",__FILE__,__LINE__); for(unsigned int i = 0; i < models.size(); i++){ +printf("%s::%i\n",__FILE__,__LINE__); std::vector internal_masks = internal[i]; std::vector external_masks = external[i]; std::vector dynamic_masks = dynamic[i]; +printf("%s::%i\n",__FILE__,__LINE__); reglib::Model * model = models[i]; - +printf("%s::%i\n",__FILE__,__LINE__); std::vector mod_po; std::vector mod_fr; std::vector mod_mm; model->getData(mod_po, mod_fr, mod_mm); - +printf("%s::%i\n",__FILE__,__LINE__); std::vector dynamic_frameid; std::vector dynamic_pixelid; - +printf("%s::%i\n",__FILE__,__LINE__); std::vector moving_frameid; std::vector moving_pixelid; - +printf("%s::%i\n",__FILE__,__LINE__); pcl::PointCloud::Ptr dynamiccloud (new pcl::PointCloud); pcl::PointCloud::Ptr movingcloud (new pcl::PointCloud); pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - +printf("%s::%i\n",__FILE__,__LINE__); for(unsigned int j = 0; j < mod_fr.size(); j++){ +printf("%s::%i\n",__FILE__,__LINE__); reglib::RGBDFrame * frame = mod_fr[j]; Eigen::Matrix4d p = mod_po[j]; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); @@ -742,19 +746,20 @@ void processMetaroom(std::string path){ } } } - - if(visualization_lvl > 0 && cloud->points.size() > 0){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - viewer->spin(); - } - +printf("%s::%i\n",__FILE__,__LINE__); +// if(visualization_lvl > 0 && cloud->points.size() > 0){ +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); +// viewer->spin(); +// } +printf("%s::%i\n",__FILE__,__LINE__); printf("dynamiccloud: %i\n",dynamiccloud->points.size()); if(dynamiccloud->points.size() > 0){ + printf("%s::%i\n",__FILE__,__LINE__); // Creating the KdTree object for the search method of the extraction pcl::search::KdTree::Ptr dynamictree (new pcl::search::KdTree); dynamictree->setInputCloud (dynamiccloud); - +printf("%s::%i\n",__FILE__,__LINE__); std::vector dynamic_indices; pcl::EuclideanClusterExtraction dynamic_ec; dynamic_ec.setClusterTolerance (0.02); // 2cm @@ -763,7 +768,7 @@ void processMetaroom(std::string path){ dynamic_ec.setSearchMethod (dynamictree); dynamic_ec.setInputCloud (dynamiccloud); dynamic_ec.extract (dynamic_indices); - +printf("%s::%i\n",__FILE__,__LINE__); for (unsigned int d = 0; d < dynamic_indices.size(); d++){ std::vector< std::vector > inds; inds.resize(mod_fr.size()); @@ -774,13 +779,13 @@ void processMetaroom(std::string path){ inds[dynamic_frameid[pid]].push_back(dynamic_pixelid[pid]); cloud_cluster->points.push_back(dynamiccloud->points[pid]); } - +printf("%s::%i\n",__FILE__,__LINE__); char buf [1024]; sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),d); QFile file(buf); if (file.exists()){file.remove();} if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); @@ -877,21 +882,29 @@ void processMetaroom(std::string path){ delete xmlWriter; } } - + printf("%s::%i\n",__FILE__,__LINE__); } + printf("%s::%i\n",__FILE__,__LINE__); } + printf("%s::%i\n",__FILE__,__LINE__); fullmodel->fullDelete(); + printf("%s::%i\n",__FILE__,__LINE__); delete fullmodel; + printf("%s::%i\n",__FILE__,__LINE__); // delete bgmassreg; delete reg; + printf("%s::%i\n",__FILE__,__LINE__); delete mu; + printf("%s::%i\n",__FILE__,__LINE__); + printf("publishing file %s to %s\n",path.c_str(),outtopic.c_str()); std_msgs::String msg; msg.data = path; out_pub.publish(msg); ros::spinOnce(); + printf("%s::%i\n",__FILE__,__LINE__); } void chatterCallback(const std_msgs::String::ConstPtr& msg){ @@ -1014,6 +1027,7 @@ std::vector loadModels(std::string path){ } void addModelToModelServer(reglib::Model * model){ + printf("addModelToModelServer\n"); model_pub.publish(quasimodo_brain::getModelMSG(model)); ros::spinOnce(); } @@ -1032,16 +1046,9 @@ void sendCallback(const std_msgs::String::ConstPtr& msg){ sendMetaroomToServer(msg->data); } -//double normalCFD(double value) -//{ -// return 0.5 * erfc(-value * M_SQRT1_2); -//} int main(int argc, char** argv){ -//for(double v = -4; v <= 4; v+= 0.01){ -// if(v >= 0){printf(" ");} -// printf("%6.6f : %6.6f\n",v,normalCFD(v)); -//}exit(0); + const rlim_t kStackSize = 256 * 1024 * 1024; // min stack size = 256 MB struct rlimit rl; unsigned long result; @@ -1069,16 +1076,9 @@ int main(int argc, char** argv){ int inputstate = 0; for(int i = 1; i < argc;i++){ printf("input: %s\n",argv[i]); - if(std::string(argv[i]).compare("-file") == 0){ inputstate = 2;} - else if(std::string(argv[i]).compare("-intopic") == 0){ inputstate = 0;} + if( std::string(argv[i]).compare("-intopic") == 0){ inputstate = 0;} else if(std::string(argv[i]).compare("-outtopic") == 0){ inputstate = 1;} - else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 4;} - else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} - else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} - else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} - else if(std::string(argv[i]).compare("-sendModel") == 0){ inputstate = 8;} - else if(std::string(argv[i]).compare("-sendSub") == 0) { inputstate = 9;} - else if(std::string(argv[i]).compare("-sendTopic") == 0){ inputstate = 10;} + else if(std::string(argv[i]).compare("-file") == 0){ inputstate = 2;} else if(std::string(argv[i]).compare("-v") == 0){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0.5, 0, 0.5); @@ -1086,11 +1086,32 @@ int main(int argc, char** argv){ viewer->initCameraParameters (); visualization_lvl = 1; inputstate = 3; - }else if(inputstate == 0){ + } + else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 4;} + else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} + else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} + else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} + else if(std::string(argv[i]).compare("-sendModel") == 0){ inputstate = 8;} + else if(std::string(argv[i]).compare("-sendSub") == 0) { inputstate = 9;} + else if(std::string(argv[i]).compare("-sendTopic") == 0){ inputstate = 10;} + else if(inputstate == 0){ + if(sendsubs.size() == 0){ + model_pub = n.advertise(modelouttopic, 1000); + sendsubs.push_back(n.subscribe(std::string(argv[i]), 1000, sendCallback)); + } out_pub = n.advertise(outtopic, 1000); segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); }else if(inputstate == 1){ outtopic = std::string(argv[i]); + }else if(inputstate == 2){ + out_pub = n.advertise(outtopic, 1000); + if(sendsubs.size() == 0){ + printf("creating model pub: %s\n",modelouttopic.c_str()); + model_pub = n.advertise(modelouttopic, 1000); + printf("creating sendsubs %s\n",modelouttopic.c_str()); + sendsubs.push_back(n.subscribe(outtopic, 1000, sendCallback)); + } + processMetaroom(std::string(argv[i])); }else if(inputstate == 3){ visualization_lvl = atoi(argv[i]); }else if(inputstate == 4){ @@ -1101,10 +1122,6 @@ int main(int argc, char** argv){ posepath = std::string(argv[i]); }else if(inputstate == 7){ sweepPoses = readPoseXML(std::string(argv[i])); - //exit(0); - }else if(inputstate == 2){ - out_pub = n.advertise(outtopic, 1000); - processMetaroom(std::string(argv[i])); }else if(inputstate == 8){ model_pub = n.advertise(modelouttopic, 1000); sendMetaroomToServer(std::string(argv[i])); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index d898447e..50bfb60b 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2083,7 +2083,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int b float px = ax/(ax+bx); float current = 0; if(!frame->det_dilate.data[ind]){ current = (1-px) * float(maxima_dxdata[ind]);} - else{ current = std::max(float(1-probs[probs.size()-2][ind]),0.99f*(1-px) * float(maxima_dxdata[ind]));} + else{ current = std::max(float(1-probs[probs.size()-2][ind]),0.8f*(1-px) * float(maxima_dxdata[ind]));} probX = 1-current; } @@ -2098,7 +2098,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int b float py = ay/(ay+by); float current = 0; if(!frame->det_dilate.data[ind]){ current = (1-py) * float(maxima_dydata[ind]);} - else{ current = std::max(float(1-probs[probs.size()-1][ind]),0.9f*(1-py) * float(maxima_dydata[ind]));} + else{ current = std::max(float(1-probs[probs.size()-1][ind]),0.8f*(1-py) * float(maxima_dydata[ind]));} probY = 1-current; } @@ -2281,9 +2281,6 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & } } - std::vector inds; - std::vector distances; - for(unsigned int src_w = 0; src_w < src_width;src_w++){ for(unsigned int src_h = 0; src_h < src_height;src_h++){ int src_ind = src_h*src_width+src_w; @@ -2338,8 +2335,8 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - if(dst_detdata[dst_ind] != 0){continue;} - if(src_detdata[src_ind] != 0){continue;} +// if(dst_detdata[dst_ind] != 0){continue;} +// if(src_detdata[src_ind] != 0){continue;} d = fabs(d); if(tz > dst_z){d = -d;} @@ -2348,18 +2345,24 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & nvec.push_back(1-surface_angle); }else{ double noise = 0.02; + double noise_angle = 0.025; double p_overlap = exp(-0.5*(d*d)/(noise*noise)); + double p_overlap_angle = exp(-0.5*((1-surface_angle)*(1-surface_angle))/(noise_angle*noise_angle)); p_overlap = std::min(0.9,p_overlap); double p_occlusion = (1.0-p_overlap)*normalCFD(d/noise); - double noise_angle = 0.05; - double p_overlap_angle = exp(-0.5*((1-surface_angle)*(1-surface_angle))/(noise_angle*noise_angle)); p_overlap *= p_overlap_angle; - overlaps[src_ind] = std::max(overlaps[src_ind],p_overlap); - occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); + if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0){ interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); - interframe_connectionStrength[offset1+src_ind].push_back(-log(1-overlaps[src_ind])); + interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap)); } + + if(dst_detdata[dst_ind] != 0){continue;} + if(src_detdata[src_ind] != 0){continue;} + + overlaps[src_ind] = std::max(overlaps[src_ind],p_overlap); + occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); + } // if(fabs(d) < 0.02){//If close, according noises, and angle of the surfaces similar: FUSE // if(surface_angle > 0.9){ @@ -2766,15 +2769,10 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } - - - printf("dynamic_frameConnections: %i\n",dynamic_frameConnections); printf("dynamic_interframeConnections: %i\n",dynamic_interframeConnections); -printf("LINE: %i\n",__LINE__); start_inf = getTime(); gc::Graph * dynamic_g = new gc::Graph(nr_dynamic,dynamic_frameConnections+dynamic_interframeConnections); -printf("LINE: %i\n",__LINE__); for(unsigned long i = 0; i < dyn_ind.size();i++){ dynamic_g -> add_node(); double p_fg = priors[3*dyn_ind[i]+0]; @@ -2791,7 +2789,7 @@ printf("LINE: %i\n",__LINE__); double weightBG = -log(p_bg); dynamic_g -> add_tweights( i, weightFG, weightBG ); } -printf("LINE: %i\n",__LINE__); + for(unsigned long i = 0; i < frames.size(); i++){ int offset = offsets[i]; Camera * camera = frames[i]->camera; @@ -2827,7 +2825,6 @@ printf("LINE: %i\n",__LINE__); } } } -printf("LINE: %i\n",__LINE__); for(unsigned long i = 0; i < current_point;i++){ if(labels[i] == dynamic_label){ @@ -2839,7 +2836,6 @@ printf("LINE: %i\n",__LINE__); } } } -printf("LINE: %i\n",__LINE__); interframe_connectionId.clear(); interframe_connectionStrength.clear(); @@ -2861,103 +2857,141 @@ printf("LINE: %i\n",__LINE__); std::vector staticdata; std::vector dynamicdata; std::vector movingdata; - pcl::PointCloud::Ptr staticcloud (new pcl::PointCloud); pcl::PointCloud::Ptr dynamiccloud (new pcl::PointCloud); pcl::PointCloud::Ptr movingcloud (new pcl::PointCloud); - for(unsigned int i = 0; i < current_point; i++){ - //pcl::PointXYZRGBNormal p = cloud->points[i]; - if(labels[i] == 0){ - staticdata.push_back(i); - staticcloud->points.push_back(cloud->points[i]); - } - if(labels[i] == 1){ - dynamicdata.push_back(i); - dynamiccloud->points.push_back(cloud->points[i]); - } + unsigned long current = 0; + for(unsigned long f = 0; f < frames.size(); f++){ + Camera * camera = frames[f]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + unsigned short * depthdata = (unsigned short *)(frames[f]->depth.data); - if(labels[i] == 2){ - movingdata.push_back(i); - movingcloud->points.push_back(cloud->points[i]); + for(int j = 0; j < width*height; j++){ + if(depthdata[j] != 0){ + if(labels[current] == 0){ + staticdata.push_back(current); + staticcloud->points.push_back(cloud->points[current]); + } + + if(labels[current] == 1){ + dynamicdata.push_back(current); + dynamiccloud->points.push_back(cloud->points[current]); + } + + if(labels[current] == 2){ + movingdata.push_back(current); + movingcloud->points.push_back(cloud->points[current]); + } + } + current++; } } + start_inf = getTime(); + pcl::search::KdTree::Ptr dynamictree (new pcl::search::KdTree); dynamictree->setInputCloud (dynamiccloud); std::vector dynamic_indices; pcl::EuclideanClusterExtraction dynamic_ec; - dynamic_ec.setClusterTolerance (0.03); // 2cm + dynamic_ec.setClusterTolerance (0.05); // 5cm dynamic_ec.setMinClusterSize (1); dynamic_ec.setMaxClusterSize (250000000); dynamic_ec.setSearchMethod (dynamictree); dynamic_ec.setInputCloud (dynamiccloud); dynamic_ec.extract (dynamic_indices); -printf("dynamic\n"); + end_inf = getTime(); + printf("dynamic cluster time: %10.10fs\n",end_inf-start_inf); for (unsigned int d = 0; d < dynamic_indices.size(); d++){ -// std::vector< std::vector > inds; -// inds.resize(mod_fr.size()); - double score = 0; pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + double sum0 = 0; + double sum1 = 0; + double sum2 = 0; for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ int pid = dynamic_indices[d].indices[ind]; int dind = dynamicdata[pid]; score += priors[3*dind+1] -0.5*(priors[3*dind+0]+priors[3*dind+2]); + + sum0 += priors[3*dind+0]; + sum1 += priors[3*dind+1]; + sum2 += priors[3*dind+2]; + cloud_cluster->points.push_back(dynamiccloud->points[pid]); } - printf("score: %f\n",score); - if(score < 100){ + double avg = score/double(dynamic_indices[d].indices.size()); + +// printf("score: %f avg: %f\n",score,avg); +// printf("sum0: %f sum1: %f sum2: %f\n",sum0,sum1,sum2); + if(score > 100 && avg > 0.075){ for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ - labels[dynamicdata[dynamic_indices[d].indices[ind]]] = 0; + labels[dynamicdata[dynamic_indices[d].indices[ind]]] = 3; } - }else{ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); - viewer->spin(); } + + +// if(cloud_cluster->points.size() > 500){ +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); +// viewer->spin(); +// } } + start_inf = getTime(); pcl::search::KdTree::Ptr movingtree (new pcl::search::KdTree); movingtree->setInputCloud (movingcloud); std::vector moving_indices; pcl::EuclideanClusterExtraction moving_ec; - moving_ec.setClusterTolerance (0.03); // 2cm + moving_ec.setClusterTolerance (0.05); // 5cm moving_ec.setMinClusterSize (1); moving_ec.setMaxClusterSize (250000000); moving_ec.setSearchMethod (movingtree); moving_ec.setInputCloud (movingcloud); moving_ec.extract (moving_indices); - printf("moving\n"); + end_inf = getTime(); + printf("moving cluster time: %10.10fs\n",end_inf-start_inf); + for (unsigned int d = 0; d < moving_indices.size(); d++){ double score = 0; pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + double sum0 = 0; + double sum1 = 0; + double sum2 = 0; for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ int pid = moving_indices[d].indices[ind]; int dind = movingdata[pid]; - score += priors[3*dind+2] -0.5*(priors[3*dind+0]+priors[3*dind+1]); + score += priors[3*dind+0] -0.5*(priors[3*dind+1]+priors[3*dind+2]); + + sum0 += priors[3*dind+0]; + sum1 += priors[3*dind+1]; + sum2 += priors[3*dind+2]; + cloud_cluster->points.push_back(movingcloud->points[pid]); } - printf("score: %f\n",score); - if(score < 100){ + double avg = score/double(moving_indices[d].indices.size()); +// printf("score: %f avg: %f\n",score,avg); +// printf("sum0: %f sum1: %f sum2: %f\n",sum0,sum1,sum2); + if(score > 100 && avg > 0.075){ for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ - labels[movingdata[moving_indices[d].indices[ind]]] = 0; + labels[movingdata[moving_indices[d].indices[ind]]] = 4; } - }else{ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); - viewer->spin(); } + +// if(cloud_cluster->points.size() > 500){ +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); +// viewer->spin(); +// } } -// printf("nr_moving: %i\n",nr_moving); - unsigned long current = 0; + current = 0; for(unsigned long i = 0; i < frames.size(); i++){ Camera * camera = frames[i]->camera; const unsigned int width = camera->width; @@ -2971,96 +3005,64 @@ printf("dynamic\n"); d.create(height,width,CV_8UC1); unsigned char * ddata = (unsigned char*)d.data; for(int j = 0; j < width*height; j++){ - mdata[j] = labels[current]==2; - ddata[j] = labels[current]==1; + mdata[j] = 255*(labels[current]==4); + ddata[j] = 255*(labels[current]==3); current++; } - movemask.push_back(m); dynmask.push_back(d); } - -if(debugg){ - printf("3-class\n"); - for(unsigned int i = 0; i < current_point; i++){ - cloud->points[i].r = priors[3*i+0]*255.0; - cloud->points[i].g = priors[3*i+1]*255.0; - cloud->points[i].b = priors[3*i+2]*255.0; - } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - -// printf("2-class\n"); -// for(unsigned int i = 0; i < current_point; i++){ -// cloud->points[i].r = priors[3*i+2]*255.0; -// cloud->points[i].g = (priors[3*i+0]+priors[3*i+1])*255.0; -// cloud->points[i].b = 0; -// } -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); -// viewer->spin(); - - printf("results-class\n"); - for(unsigned int i = 0; i < current_point; i++){ - if(labels[i] == 0){ - cloud->points[i].r = 0; - cloud->points[i].g = 0; - cloud->points[i].b = 255; - } - - if(labels[i] == 1){ - cloud->points[i].r = 0; - cloud->points[i].g = 255; - cloud->points[i].b = 0; - } - - if(labels[i] == 2){ - cloud->points[i].r = 255; - cloud->points[i].g = 0; - cloud->points[i].b = 0; + if(debugg){ + printf("3-class\n"); + for(unsigned int i = 0; i < current_point; i++){ + cloud->points[i].r = priors[3*i+0]*255.0; + cloud->points[i].g = priors[3*i+1]*255.0; + cloud->points[i].b = priors[3*i+2]*255.0; } - } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - -} - - - - + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + printf("results-class\n"); + for(unsigned int i = 0; i < current_point; i++){ + if(labels[i] == 0){ + cloud->points[i].r = 0; + cloud->points[i].g = 0; + cloud->points[i].b = 255; + } + if(labels[i] == 1){ + cloud->points[i].r = 0; + cloud->points[i].g = 0; + cloud->points[i].b = 0; + } -//return labels; -// -// std::vector< std::vector > frame_connectionId; -// std::vector< std::vector > frame_connectionStrength; + if(labels[i] == 2){ + cloud->points[i].r = 0; + cloud->points[i].g = 0; + cloud->points[i].b = 0; + } -// frame_prior.resize(nr_pixels); -// frame_connectionId.resize(nr_pixels); -// frame_connectionStrength.resize(nr_pixels); + if(labels[i] == 3){ + cloud->points[i].r = 0; + cloud->points[i].g = 255; + cloud->points[i].b = 0; + } -// for(unsigned int i = 0; i < interframe_connectionId.size(); i++){ -// for(unsigned int j = 0; j < interframe_connectionId[i].size(); j++){ -// connectionId[i].push_back(interframe_connectionId[i][j]); -// connectionStrength[i].push_back(interframe_connectionStrength[i][j]); -// } -// } -// std::vector improvedglobal_labels = doInference(prior,connectionId,connectionStrength); + if(labels[i] == 4){ + cloud->points[i].r = 255; + cloud->points[i].g = 0; + cloud->points[i].b = 0; + } + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); -// delete[] estimates; -// delete[] priors_weight; -delete[] priors; -// delete[] points; -// delete[] colors; -// delete[] normals; + } - //return labels; - printf("done\n"); - exit(0); + delete[] priors; } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp index a29fb6e8..83c6fd03 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp @@ -29,7 +29,9 @@ ModelUpdaterBasicFuse::ModelUpdaterBasicFuse(Model * model_, Registration * regi show_scoring = false;//fuse scoring show } -ModelUpdaterBasicFuse::~ModelUpdaterBasicFuse(){} +ModelUpdaterBasicFuse::~ModelUpdaterBasicFuse(){ + printf("deleting ModelUpdaterBasicFuse\n"); +} FusionResults ModelUpdaterBasicFuse::registerModel(Model * model2, Eigen::Matrix4d guess, double uncertanity){ if(model->points.size() > 0 && model2->points.size() > 0){ From 67d2d03599d736ae7d14a094261f4d023720f485 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Fri, 2 Sep 2016 16:21:38 +0200 Subject: [PATCH 082/255] Added a couple of new methods to vocabulary_tree to find the voc distance between two features --- .../k_means_tree/impl/vocabulary_tree.hpp | 16 ++++++++++++++++ .../include/vocabulary_tree/vocabulary_tree.h | 4 +++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp b/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp index 7fbce3f3..dd162a8c 100644 --- a/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp +++ b/dynamic_object_retrieval/k_means_tree/impl/vocabulary_tree.hpp @@ -839,3 +839,19 @@ void vocabulary_tree::load(Archive& archive) cout << "Finished loading vocabulary_tree" << endl; } */ + +template +double vocabulary_tree::compute_distance(const std::map& vec1, double norm1, + const std::map& vec2, double norm2) const +{ + double score = 0.0; + for (const pair& n : vec1) { + if (vec2.count(n.first) == 0) { + continue; + } + // actually we don't need to check the depth of the node since that is already handled by compute_query_vector: GREAT! + score += std::min(n.second, vec2.at(n.first)); + } + score = 1.0 - score/std::max(norm1, norm2); + return score; +} diff --git a/dynamic_object_retrieval/k_means_tree/include/vocabulary_tree/vocabulary_tree.h b/dynamic_object_retrieval/k_means_tree/include/vocabulary_tree/vocabulary_tree.h index 4a2564e9..5d3a7f1e 100644 --- a/dynamic_object_retrieval/k_means_tree/include/vocabulary_tree/vocabulary_tree.h +++ b/dynamic_object_retrieval/k_means_tree/include/vocabulary_tree/vocabulary_tree.h @@ -76,7 +76,6 @@ class vocabulary_tree : public k_means_tree { double pexp(const double v) const; double proot(const double v) const; - double compute_query_vector(std::map& query_id_freqs, CloudPtrT& query_cloud); void compute_query_vector(std::map& query_id_freqs, CloudPtrT& query_cloud); double compute_query_vector(std::map >& query_id_freqs, CloudPtrT& query_cloud); void source_freqs_for_node(std::map& source_id_freqs, node* n) const; @@ -129,6 +128,9 @@ class vocabulary_tree : public k_means_tree { void compute_query_index_vector(std::map& query_index_freqs, CloudPtrT& query_cloud, std::map& mapping); vocabulary_vector compute_query_index_vector(CloudPtrT& query_cloud, std::map &mapping); + double compute_query_vector(std::map& query_id_freqs, CloudPtrT& query_cloud); + double compute_distance(const std::map& vec1, double norm1, const std::map& vec2, double norm2) const; + /* template void save(Archive& archive) const; template void load(Archive& archive); From 522283844223bb987a84d9a6e4b0a8fe8be1866f Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Fri, 2 Sep 2016 16:28:17 +0200 Subject: [PATCH 083/255] Added the service for retrieval and added some new messages --- quasimodo/quasimodo_msgs/CMakeLists.txt | 3 + quasimodo/quasimodo_msgs/msg/model.msg | 1 + quasimodo/quasimodo_msgs/msg/model_array.msg | 1 + .../quasimodo_msgs/msg/retrieval_query.msg | 1 + .../msg/retrieval_query_array.msg | 1 + .../quasimodo_msgs/msg/retrieval_result.msg | 1 + .../srv/model_to_retrieval_query.srv | 3 + .../src/quasimodo_retrieval_node.cpp | 172 ++++-------------- 8 files changed, 44 insertions(+), 139 deletions(-) create mode 100644 quasimodo/quasimodo_msgs/msg/model_array.msg create mode 100644 quasimodo/quasimodo_msgs/msg/retrieval_query_array.msg create mode 100644 quasimodo/quasimodo_msgs/srv/model_to_retrieval_query.srv diff --git a/quasimodo/quasimodo_msgs/CMakeLists.txt b/quasimodo/quasimodo_msgs/CMakeLists.txt index cb186800..a3f5f1aa 100644 --- a/quasimodo/quasimodo_msgs/CMakeLists.txt +++ b/quasimodo/quasimodo_msgs/CMakeLists.txt @@ -52,6 +52,8 @@ add_message_files( retrieval_query_result.msg SOMA2Object.msg fused_world_state_object.msg + model_array.msg + retrieval_query_array.msg ) ## Generate services in the 'srv' folder @@ -69,6 +71,7 @@ add_service_files( transform_cloud.srv mask_pointclouds.srv insert_model.srv + model_to_retrieval_query.srv ) ## Generate actions in the 'action' folder diff --git a/quasimodo/quasimodo_msgs/msg/model.msg b/quasimodo/quasimodo_msgs/msg/model.msg index 72ad70a4..a6de3242 100644 --- a/quasimodo/quasimodo_msgs/msg/model.msg +++ b/quasimodo/quasimodo_msgs/msg/model.msg @@ -3,3 +3,4 @@ geometry_msgs/Pose[] local_poses rgbd_frame[] frames sensor_msgs/Image[] masks sensor_msgs/PointCloud2[] clouds +geometry_msgs/Pose[] global_pose diff --git a/quasimodo/quasimodo_msgs/msg/model_array.msg b/quasimodo/quasimodo_msgs/msg/model_array.msg new file mode 100644 index 00000000..6c4d29ad --- /dev/null +++ b/quasimodo/quasimodo_msgs/msg/model_array.msg @@ -0,0 +1 @@ +quasimodo_msgs/model[] models diff --git a/quasimodo/quasimodo_msgs/msg/retrieval_query.msg b/quasimodo/quasimodo_msgs/msg/retrieval_query.msg index bef27724..cff71091 100644 --- a/quasimodo/quasimodo_msgs/msg/retrieval_query.msg +++ b/quasimodo/quasimodo_msgs/msg/retrieval_query.msg @@ -5,6 +5,7 @@ sensor_msgs/PointCloud2 cloud sensor_msgs/Image image sensor_msgs/Image depth sensor_msgs/Image mask +geometry_msgs/Pose global_pose sensor_msgs/CameraInfo camera int32 number_query geometry_msgs/Transform room_transform diff --git a/quasimodo/quasimodo_msgs/msg/retrieval_query_array.msg b/quasimodo/quasimodo_msgs/msg/retrieval_query_array.msg new file mode 100644 index 00000000..4a112f10 --- /dev/null +++ b/quasimodo/quasimodo_msgs/msg/retrieval_query_array.msg @@ -0,0 +1 @@ +quasimodo_msgs/retrieval_query[] queries diff --git a/quasimodo/quasimodo_msgs/msg/retrieval_result.msg b/quasimodo/quasimodo_msgs/msg/retrieval_result.msg index ce37b62c..37f3faba 100644 --- a/quasimodo/quasimodo_msgs/msg/retrieval_result.msg +++ b/quasimodo/quasimodo_msgs/msg/retrieval_result.msg @@ -6,3 +6,4 @@ image_array[] retrieved_masks string_array[] retrieved_image_paths float64[] retrieved_distance_scores int_array[] segment_indices +geometry_msgs/Pose[] global_poses diff --git a/quasimodo/quasimodo_msgs/srv/model_to_retrieval_query.srv b/quasimodo/quasimodo_msgs/srv/model_to_retrieval_query.srv new file mode 100644 index 00000000..5e1de100 --- /dev/null +++ b/quasimodo/quasimodo_msgs/srv/model_to_retrieval_query.srv @@ -0,0 +1,3 @@ +quasimodo_msgs/model model +--- +quasimodo_msgs/retrieval_query query diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index e6038053..e87c2e94 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -19,6 +19,7 @@ #include "quasimodo_msgs/retrieval_query_result.h" #include "quasimodo_msgs/retrieval_query.h" #include "quasimodo_msgs/simple_query_cloud.h" +#include "quasimodo_msgs/query_cloud.h" #include #include #include @@ -134,7 +135,7 @@ class retrieval_node { pubs[i] = n.advertise(string("/retrieval_cloud/") + to_string(i), 1, true); } sub = n.subscribe(retrieval_input, 10, &retrieval_node::run_retrieval, this); - //service = n.advertiseService("/query_normal_cloud", &retrieval_node::service_callback, this); + service = n.advertiseService("/quasimodo_retrieval_service", &retrieval_node::service_callback, this); /* vector folder_xmls = semantic_map_load_utilties::getSweepXmls(data_path.string(), true); for (const string& xml : folder_xmls) { @@ -406,130 +407,6 @@ class retrieval_node { std::cout << "Number of features: " << pfhrgb_cloud.size() << std::endl; } - /* - bool service_callback(quasimodo_msgs::simple_query_cloud::Request& req, - quasimodo_msgs::simple_query_cloud::Response& res) - { - // we assume that req.query_cloud contains a cloud with PointXYZRGBNormal - cout << "Received query msg..." << endl; - - pcl::PointCloud::Ptr normal_cloud(new pcl::PointCloud); - pcl::fromROSMsg(req.query_cloud, *normal_cloud); - - CloudT::Ptr cloud(new CloudT); - NormalCloudT::Ptr normals(new NormalCloudT); - cloud->reserve(normal_cloud->size()); normals->reserve(normal_cloud->size()); - for (const pcl::PointXYZRGBNormal& pn : normal_cloud->points) { - PointT p; p.getVector3fMap() = pn.getVector3fMap(); p.rgba = pn.rgba; - NormalT n; n.getNormalVector3fMap() = pn.getNormalVector3fMap(); - cloud->push_back(p); normals->push_back(n); - } - - HistCloudT::Ptr features(new HistCloudT); - CloudT::Ptr keypoints(new CloudT); - cout << "Computing features..." << endl; - test_compute_features(features, keypoints, cloud, normals, false, true); - cout << "Done computing features..." << endl; - - vector retrieved_clouds; - vector sweep_paths; - // we should retrieve more than we want and only keep the results with valid files on the system - // FIXME: add support for grouped vocabulary here also, - // all of the message types should support it, just a matter - // of going from the native types to ros types, vis server should also support it - // make the few thing which might be dependent templatized functions - auto results = dynamic_object_retrieval::query_reweight_vocabulary((vocabulary_tree&)vt, features, 200, vocabulary_path, summary); - - // This is just to make sure that we have valid results even when some meta rooms have been deleted - size_t counter = 0; - while (counter < number_query) { - // assume that everything in mongodb is kept - if (results.first[counter].first.stem().string() == "surfel_map") { - ++counter; - continue; - } - if (!boost::filesystem::exists(results.first[counter].first)) { - auto iter = std::find_if(results.first.begin()+counter, results.first.end(), [](const pair::result_type>& res) { - return boost::filesystem::exists(res.first); - }); - if (iter == results.first.end()) { - break; - } - std::swap(results.first[counter], *iter); - } - else { - ++counter; - } - } - results.first.resize(counter); - - // the sweep paths are of the form /path/to/room.xml, which do not exist for mongodb results - tie(retrieved_clouds, sweep_paths) = benchmark_retrieval::load_retrieved_clouds(results.first); - - cout << "Query cloud size: " << cloud->size() << endl; - for (CloudT::Ptr& c : retrieved_clouds) { - cout << "Retrieved cloud size: " << c->size() << endl; - } - - vector scores; - vector indices; - for (auto s : results.first) { - boost::filesystem::path segment_path = s.first; - string name = segment_path.stem().string(); - if (name == "surfel_map") { // for the mongodb results - indices.push_back(-1); - } - else { // for the metric map results - size_t last_index = name.find_last_not_of("0123456789"); - int index = stoi(name.substr(last_index + 1)); - indices.push_back(index); - } - scores.push_back(s.second.score); - } - - vector, Eigen::aligned_allocator > initial_poses; - vector > masks(retrieved_clouds.size()); - vector > images(retrieved_clouds.size()); - vector > depths(retrieved_clouds.size()); - vector > paths(retrieved_clouds.size()); - Eigen::Matrix3f K; - // The new parameters in surfelize_it: 528, 525, 317, 245 - // The old parameters in surfelize_it: 540, 540, 320, 240 - - // The old parameters in this node: - // K << 525.0f, 0.0f, 319.5f, 0.0f, 525.0f, 239.5f, 0.0f, 0.0f, 1.0f; - - // Just for johan to test: - K << 540.0f, 0.0f, 320.0f, 0.0f, 540.0f, 240.0f, 0.0f, 0.0f, 1.0f; - for (int i = 0; i < retrieved_clouds.size(); ++i) { - vector inds; - vector > sweep_transforms; - if (indices[i] == -1) { // special case for mongodb result - tie(masks[i], images[i], depths[i], inds) = generate_images_for_mongodb_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms); - } - else { - auto sweep_data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); - for (tf::StampedTransform& t : sweep_data.vIntermediateRoomCloudTransformsRegistered) { - Eigen::Affine3d e; - tf::transformTFToEigen(t, e); - sweep_transforms.push_back(e.inverse().matrix().cast()); - } - tie(masks[i], images[i], depths[i], inds) = generate_images_for_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms); - - } - for (int j = 0; j < inds.size(); ++j) { - paths[i].push_back(sweep_paths[i].string() + " " + to_string(inds[j])); - } - } - - res.result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices); - sensor_msgs::Image img_msg = construct_results_image(images); - img_pub.publish(img_msg); - - cout << "Finished retrieval..." << endl; - } - */ - // to have this templated is totally unnecessary but whatever void prune_results(std::pair::result_type> >, std::vector::result_type> > >& results) @@ -585,12 +462,12 @@ class retrieval_node { return path[0]; } - void run_retrieval(const quasimodo_msgs::retrieval_query::ConstPtr& query_msg) //const string& sweep_xml) + bool retrieval_implementation(const quasimodo_msgs::retrieval_query& query, quasimodo_msgs::retrieval_result& result, geometry_msgs::Transform& query_room_transform) { - cout << "Received query msg of kind " << query_msg->query_kind << "..." << endl; + cout << "Received query msg of kind " << query.query_kind << "..." << endl; dynamic_object_retrieval::uri_kind kind; - switch (query_msg->query_kind) { + switch (query.query_kind) { case quasimodo_msgs::retrieval_query::MONGODB_QUERY: kind = dynamic_object_retrieval::uri_kind::mongodb; break; @@ -601,12 +478,12 @@ class retrieval_node { kind = dynamic_object_retrieval::uri_kind::all; break; default: - cout << "Invalid quasimodo_msgs::retrieval_query::query_kind option: " << query_msg->query_kind << endl; - return; + cout << "Invalid quasimodo_msgs::retrieval_query::query_kind option: " << query.query_kind << endl; + return false; } pcl::PointCloud::Ptr normal_cloud(new pcl::PointCloud); - pcl::fromROSMsg(query_msg->cloud, *normal_cloud); + pcl::fromROSMsg(query.cloud, *normal_cloud); CloudT::Ptr cloud(new CloudT); NormalCloudT::Ptr normals(new NormalCloudT); cloud->reserve(normal_cloud->size()); normals->reserve(normal_cloud->size()); @@ -617,7 +494,7 @@ class retrieval_node { } image_geometry::PinholeCameraModel cam_model; - cam_model.fromCameraInfo(query_msg->camera); + cam_model.fromCameraInfo(query.camera); cv::Matx33d cvK = cam_model.intrinsicMatrix(); Eigen::Matrix3f K = Eigen::Map(cvK.val).cast(); K << 528.0f, 0.0f, 317.0f, 0.0f, 525.0f, 245.0f, 0.0f, 0.0f, 1.0f; // surfelize_it intrinsics @@ -722,23 +599,40 @@ class retrieval_node { } } - //cv::Mat full_query_image = benchmark_retrieval::sweep_get_rgb_at(sweep_xml, scan_index); - quasimodo_msgs::retrieval_query_result result; - result.query = *query_msg; - tf::transformTFToMsg(room_transform, result.query.room_transform); - result.result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices); + + tf::transformTFToMsg(room_transform, query_room_transform); + result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices); for (int i = 0; i < retrieved_clouds.size(); ++i) { - sensor_msgs::PointCloud2 cloud_msg = result.result.retrieved_clouds[i]; + sensor_msgs::PointCloud2 cloud_msg = result.retrieved_clouds[i]; cloud_msg.header.stamp = ros::Time::now(); cloud_msg.header.frame_id = "/map"; pubs[i].publish(cloud_msg); } - pub.publish(result); + sensor_msgs::Image img_msg = construct_results_image(images); img_pub.publish(img_msg); cout << "Finished retrieval..." << endl; + + return true; + } + + bool service_callback(quasimodo_msgs::query_cloud::Request& req, + quasimodo_msgs::query_cloud::Response& res) + { + geometry_msgs::Transform query_room_transform; + return retrieval_implementation(req.query, res.result, query_room_transform); + } + + void run_retrieval(const quasimodo_msgs::retrieval_query::ConstPtr& query_msg) //const string& sweep_xml) + { + quasimodo_msgs::retrieval_query_result result; + geometry_msgs::Transform query_room_transform; + retrieval_implementation(*query_msg, result.result, query_room_transform); + result.query = *query_msg; + result.query.room_transform = query_room_transform; + pub.publish(result); } void convert_to_img_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image) From 9fbcd9b9e70de3abf09dde443c719b3d30df1840 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 2 Sep 2016 23:10:31 +0200 Subject: [PATCH 084/255] improvements to segmentation --- quasimodo/quasimodo_brain/CMakeLists.txt | 2 +- quasimodo/quasimodo_brain/src/Util/Util.cpp | 18 +- quasimodo/quasimodo_brain/src/modelserver.cpp | 192 +++----- .../weightfunctions/DistanceWeightFunction2.h | 4 +- .../DistanceWeightFunction2PPR2.h | 23 +- .../src/modelupdater/ModelUpdater.cpp | 446 +++++++++--------- .../DistanceWeightFunction2.cpp | 8 +- .../DistanceWeightFunction2PPR2.cpp | 193 +++++--- .../src/RobotContainer.cpp | 8 +- 9 files changed, 453 insertions(+), 441 deletions(-) diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 4fd9f819..53ca5d7b 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -116,7 +116,7 @@ target_link_libraries( robot_listener image_geometry cpp_common roscpp rosconso add_executable( modelserver src/modelserver.cpp) add_dependencies( modelserver roscpp quasimodo_msgs_generate_messages_cpp) -target_link_libraries( modelserver quasimodo_ModelDatabase image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries( modelserver quasimodo_brain_util quasimodo_ModelDatabase image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) add_executable( massregPCD src/massregPCD.cpp) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 927e8246..4f46affc 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -165,10 +165,20 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS - cam->fx = 532.158936; - cam->fy = 533.819214; - cam->cx = 310.514310; - cam->cy = 236.842039; +// cam->fx = 532.158936; +// cam->fy = 533.819214; +// cam->cx = 310.514310; +// cam->cy = 236.842039; + + image_geometry::PinholeCameraModel aCameraModel = roomData.vIntermediateRoomCloudCamParamsCorrected[i]; + //rc->initializeCamera(aCameraModel.fx(), aCameraModel.fy(), aCameraModel.cx(), aCameraModel.cy(), aCameraModel.fullResolution().width, aCameraModel.fullResolution().height); + + cam->fx = aCameraModel.fx(); + cam->fy = aCameraModel.fy(); + cam->cx = aCameraModel.cx(); + cam->cy = aCameraModel.cy(); + cam->print(); + //aCameraModel.fullResolution().width, aCameraModel.fullResolution().height); // cout<<"Intermediate cloud size "<points.size()< #include +#include "Util/Util.h" + +using namespace quasimodo_brain; + bool visualization = false; bool show_db = false;//Full database show int show_init_lvl = 0;//init show @@ -93,13 +97,9 @@ double search_timeout = 30; int sweepid_counter = 0; -bool myfunction (reglib::Model * i,reglib::Model * j) { return i->frames.size() > j->frames.size(); } +int current_model_update = 0; -double getTime(){ - struct timeval start1; - gettimeofday(&start1, NULL); - return double(start1.tv_sec+(start1.tv_usec/1000000.0)); -} +bool myfunction (reglib::Model * i,reglib::Model * j) { return i->frames.size() > j->frames.size(); } quasimodo_msgs::retrieval_result sresult; bool new_search_result = false; @@ -149,7 +149,6 @@ void publishDatabasePCD(bool original_colors = false){ } void publish_history(std::vector::Ptr> history){ - for(unsigned int i = 0; i < history.size(); i++){ sensor_msgs::PointCloud2 input; pcl::toROSMsg (*history[i],input);//, *transformed_cloud); @@ -158,24 +157,6 @@ void publish_history(std::vector::Ptr> history } } -void publish_places(reglib::Model * mod){ - pcl::PointCloud::Ptr cloud = mod->getPCLcloud(1,false); - for(unsigned int i = 0; i < mod->submodels.size(); i++){ - Eigen::Matrix4d pose = mod->submodels[i]->frames.front()->pose * mod->submodels_relativeposes[i].inverse(); - pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); - pcl::transformPointCloud (*cloud, *transformed_cloud, pose); - - sensor_msgs::PointCloud2 input; - pcl::toROSMsg (*transformed_cloud,input);//, *transformed_cloud); - input.header.frame_id = "/map"; - model_places_pub.publish(input); - } -} - -void publishObject(int id){ - -} - void dumpDatabase(std::string path = "."){ char command [1024]; @@ -232,6 +213,7 @@ void retrievalCallback(const quasimodo_msgs::retrieval_query_result & qr){ int savecounter = 0; void show_sorted(){ + printf("show_sorted\n"); if(!show_db){return;} if(!visualization){return;} std::vector results; @@ -239,6 +221,9 @@ void show_sorted(){ std::sort (results.begin(), results.end(), myfunction); viewer->removeAllPointClouds(); float maxx = 0; + + + printf("pre->loop\n"); for(unsigned int i = 0; i < results.size(); i++){ pcl::PointCloud::Ptr cloud = results[i]->getPCLcloud(1, false); float meanx = 0; @@ -275,90 +260,6 @@ void show_sorted(){ //viewer->spinOnce(); } -void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp = Eigen::Affine3d::Identity()){ - int startsize = msg.local_poses.size(); - msg.local_poses.resize(startsize+model->relativeposes.size()); - msg.frames.resize(startsize+model->frames.size()); - msg.masks.resize(startsize+model->modelmasks.size()); - for(unsigned int i = 0; i < model->relativeposes.size(); i++){ - geometry_msgs::Pose pose1; - tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i])*rp, pose1); - geometry_msgs::Pose pose2; - tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose)*rp, pose2); - cv_bridge::CvImage rgbBridgeImage; - rgbBridgeImage.image = model->frames[i]->rgb; - rgbBridgeImage.encoding = "bgr8"; - cv_bridge::CvImage depthBridgeImage; - depthBridgeImage.image = model->frames[i]->depth; - depthBridgeImage.encoding = "mono16"; - cv_bridge::CvImage maskBridgeImage; - maskBridgeImage.image = model->modelmasks[i]->getMask(); - maskBridgeImage.encoding = "mono8"; - msg.local_poses[startsize+i] = pose1; - msg.frames[startsize+i].capture_time = ros::Time(); - msg.frames[startsize+i].pose = pose2; - msg.frames[startsize+i].frame_id = model->frames[i]->id; - msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); - msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); - msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg());//getMask() - } - for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ - addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp); - } -} - -quasimodo_msgs::model getModelMSG(reglib::Model * model){ - quasimodo_msgs::model msg; - msg.model_id = model->id; - addToModelMSG(msg,model); - - - return msg; -} - -reglib::Model * getModelFromMSG(quasimodo_msgs::model msg){ - reglib::Model * model = new reglib::Model(); - - for(unsigned int i = 0; i < msg.local_poses.size(); i++){ - sensor_msgs::CameraInfo camera = msg.frames[i].camera; - ros::Time capture_time = msg.frames[i].capture_time; - geometry_msgs::Pose pose = msg.frames[i].pose; - - cv_bridge::CvImagePtr rgb_ptr; - try{ rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat rgb = rgb_ptr->image; - - cv_bridge::CvImagePtr depth_ptr; - try{ depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat depth = depth_ptr->image; - - Eigen::Affine3d epose; - tf::poseMsgToEigen(pose, epose); - - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); - model->frames.push_back(frame); - - geometry_msgs::Pose pose1 = msg.local_poses[i]; - Eigen::Affine3d epose1; - tf::poseMsgToEigen(pose1, epose1); - model->relativeposes.push_back(epose1.matrix()); - - cv_bridge::CvImagePtr mask_ptr; - try{ mask_ptr = cv_bridge::toCvCopy(msg.masks[i], sensor_msgs::image_encodings::MONO8);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat mask = mask_ptr->image; - - model->modelmasks.push_back(new reglib::ModelMask(mask)); - model->modelmasks.back()->sweepid = sweepid_counter; - } - model->recomputeModelPoints(); - sweepid_counter++; - - return model; -} - std::vector getSOMA2ObjectMSGs(reglib::Model * model){ std::vector msgs; for(unsigned int i = 0; i < model->frames.size(); i++){ @@ -558,11 +459,6 @@ void call_from_thread(int i) { delete reg; } -/* -bool show_db = false;//Full database show -*/ - -int current_model_update = 0; void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, bool deleteIfFail = false){ printf("addToDB %i %i\n",int(add),int(deleteIfFail)); @@ -590,7 +486,7 @@ show_sorted(); mod = model; res = modeldatabase->search(model,3); - printf("results found: %i\n",res.size()); + printf("results found: %i\n",int(res.size())); fr_res.resize(res.size()); if(res.size() == 0){ @@ -742,16 +638,63 @@ show_sorted(); void addNewModel(reglib::Model * model){ - modeldatabase->add(model); - addToDB(modeldatabase, model,false); + printf("model: %i\n",model->frames.size()); + + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model, reg); + mu->occlusion_penalty = occlusion_penalty; + mu->massreg_timeout = massreg_timeout; + mu->viewer = viewer; + + mu->show_init_lvl = show_init_lvl;//init show + mu->show_refine_lvl = show_refine_lvl;//refine show + mu->show_scoring = show_scoring;//fuse scoring show + reg->visualizationLvl = show_reg_lvl; + + model->print(); + mu->makeInitialSetup(); + model->print(); + + delete mu; + delete reg; + + reglib::Model * newmodelHolder = new reglib::Model(); + newmodelHolder->submodels.push_back(model); + newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + newmodelHolder->last_changed = ++current_model_update; + newmodelHolder->recomputeModelPoints(); + + modeldatabase->add(newmodelHolder); + addToDB(modeldatabase, newmodelHolder,false); for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ publish_history(modeldatabase->models[i]->getHistory()); } - - show_sorted(); publishDatabasePCD(); dumpDatabase(savepath); +// modeldatabase->add(newmodelHolder); +// addToDB(modeldatabase, newmodelHolder,false); +// for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ +// publish_history(modeldatabase->models[i]->getHistory()); +// } +// /* +// for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ +// publish_history(modeldatabase->models[i]->getHistory()); +// }*/ +// show_sorted(); +// publishDatabasePCD(); +// dumpDatabase(savepath); + + +// modeldatabase->add(model); +// addToDB(modeldatabase, model,false); +// for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ +// publish_history(modeldatabase->models[i]->getHistory()); +// } +// publishDatabasePCD(); +// dumpDatabase(savepath); + // for(unsigned int m = 0; run_search && m < modeldatabase->models.size(); m++){ // printf("looking at: %i\n",int(modeldatabase->models[m]->last_changed)); @@ -1053,7 +996,8 @@ void addNewModel(reglib::Model * model){ } void modelCallback(const quasimodo_msgs::model & m){ - addNewModel(getModelFromMSG(m)); + quasimodo_msgs::model mod = m; + addNewModel(quasimodo_brain::getModelFromMSG(mod)); } bool nextNew = true; @@ -1531,7 +1475,7 @@ vector > ocs = computeOcclusionScore(models,rps); return true; } -bool fuseModels(quasimodo_msgs::fuse_models::Request & req, quasimodo_msgs::fuse_models::Response & res){} +//bool fuseModels(quasimodo_msgs::fuse_models::Request & req, quasimodo_msgs::fuse_models::Response & res){} int getdir (std::string dir, std::vector & files){ DIR *dp; @@ -1596,8 +1540,8 @@ int main(int argc, char **argv){ ros::ServiceServer service2 = n.advertiseService("index_frame", indexFrame); ROS_INFO("Ready to add use index_frame."); - ros::ServiceServer service3 = n.advertiseService("fuse_models", fuseModels); - ROS_INFO("Ready to add use fuse_models."); + //ros::ServiceServer service3 = n.advertiseService("fuse_models", fuseModels); + //ROS_INFO("Ready to add use fuse_models."); ros::ServiceServer service4 = n.advertiseService("get_model", getModel); ROS_INFO("Ready to add use get_model."); @@ -1672,6 +1616,8 @@ int main(int argc, char **argv){ } } + if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/model/out/topic", 100, modelCallback));} + if(visualization){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); viewer->addCoordinateSystem(0.1); diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h index 8a448299..35a65b6e 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h @@ -50,7 +50,9 @@ class DistanceWeightFunction2 virtual void computeModel(MatrixXd mat); virtual VectorXd getProbs(std::vector & vec); virtual VectorXd getProbs(MatrixXd mat); - virtual double getProb(double d); + + virtual double getProbInfront(double d, bool debugg = false); + virtual double getProb(double d, bool debugg = false); virtual double getNoise(); virtual double getConvergenceThreshold(); virtual bool update(); diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h index 4d171df3..703f21d8 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h @@ -26,6 +26,15 @@ using ceres::Problem; using ceres::Solver; using ceres::Solve; +class distributionFunction { + public: + + virtual void train(std::vector & hist, int nr_bins = -1); + virtual void update(); + virtual double getval(double x); + virtual double getcdf(double x); +}; + class DistanceWeightFunction2PPR2 : public DistanceWeightFunction2 { public: @@ -62,11 +71,16 @@ class DistanceWeightFunction2PPR2 : public DistanceWeightFunction2 double noiseval; double startreg; - + + bool ggd; + bool compute_infront; + std::vector prob; + std::vector infront; std::vector histogram; std::vector blur_histogram; std::vector noise; + std::vector noisecdf; int nr_refineiters; bool refine_mean; @@ -87,16 +101,21 @@ class DistanceWeightFunction2PPR2 : public DistanceWeightFunction2 bool bidir; int iter; + double histogram_mul; + double histogram_mul2; + DistanceWeightFunction2PPR2( double maxd_ = 0.25, int histogram_size_ = 25000); ~DistanceWeightFunction2PPR2(); virtual void computeModel(MatrixXd mat); virtual VectorXd getProbs(MatrixXd mat); - virtual double getProb(double d); + virtual double getProb(double d, bool debugg = false); + virtual double getProbInfront(double d, bool debugg = false); virtual double getNoise(); virtual double getConvergenceThreshold(); virtual bool update(); virtual void reset(); virtual std::string getString(); + virtual double getInd(double d, bool debugg = false); }; } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 50bfb60b..6fc5f7bb 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -619,50 +619,12 @@ void ModelUpdater::makeInitialSetup(){ return ; } - int minHessian = 400; - - // cv::SurfFeatureDetector detector( minHessian ); - // cv::SurfDescriptorExtractor extractor; - - - //cv::ORB orb = cv::ORB(250);//,1.2f, 1, 3, 0,2, cv::ORB::HARRIS_SCORE, 31); - cv::ORB orb = cv::ORB(10);//,1.2f, 1, 3, 0,2, cv::ORB::HARRIS_SCORE, 31); - // for(unsigned int i = 0; i < model->frames.size(); i++){ - // std::vector keypoints; - // cv::Mat descriptors; - // //orb(model->frames[i]->rgb, model->modelmasks[i]->getMask(), keypoints, descriptors); - // orb(model->frames[i]->rgb, cv::Mat(), keypoints, descriptors); - // model->all_keypoints.push_back(keypoints); - // model->all_descriptors.push_back(descriptors); - - // printf("keypoints: %i\n",keypoints.size()); - - - // cv::Mat descriptors_surf; - // std::vector keypoints_surf; - - // detector.detect(model->frames[i]->rgb, keypoints_surf, model->modelmasks[i]->getMask() ); - // extractor.compute( model->frames[i]->rgb, keypoints_surf, descriptors_surf ); - // model->all_keypoints.push_back(keypoints_surf); - // model->all_descriptors.push_back(descriptors_surf); - - //// printf("keypoints: %i\n",keypoints.size()); - //// cv::Mat out; - //// cv::drawKeypoints(model->frames[i]->rgb, keypoints_surf, out, cv::Scalar::all(255)); - //// cv::imshow("Kpts", out); - //// cv::waitKey(0); - // } - - // show_refine = false;//refine show - // show_reg = false;//registration show - // show_scoring = false;//fuse scoring sho - MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); massreg->timeout = 4*massreg_timeout; massreg->viewer = viewer; massreg->visualizationLvl = show_init_lvl; - massreg->maskstep = std::max(1,int(0.25*double(model->frames.size()))); + massreg->maskstep = 1;//std::max(1,int(0.25*double(model->frames.size()))); massreg->nomaskstep = std::max(5,int(0.5+0.3*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massreg->nomask = true; massreg->stopval = 0.0005; @@ -670,43 +632,7 @@ void ModelUpdater::makeInitialSetup(){ massreg->setData(model->frames,model->modelmasks); MassFusionResults mfr = massreg->getTransforms(model->relativeposes); - - /* - MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); - massreg->timeout = 4*massreg_timeout; - massreg->viewer = viewer; - massreg->visualizationLvl = show_init_lvl; - - massreg->maskstep = std::max(1,int(0.25*double(model->frames.size()))); - massreg->nomaskstep = std::max(5,int(0.5+0.5*double(model->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); - massreg->nomask = true; - massreg->stopval = 0.0001; - - //massreg->setData(model->frames,model->modelmasks); - massreg->addModelData(model, false); - std::vector p = model->submodels_relativeposes; - p.insert(p.end(), model->relativeposes.begin(), model->relativeposes.end()); - - - //MassFusionResults mfr = massreg->getTransforms(model->relativeposes); - MassFusionResults mfr = massreg->getTransforms(p); -*/ - - model->relativeposes.clear();// = mfr.poses; - model->relativeposes.insert( model->relativeposes.end(), mfr.poses.begin()+model->submodels.size(), mfr.poses.end()); - - vector > ocs = getOcclusionScores(model->relativeposes, model->frames, model->modelmasks, false, 1); - std::vector > scores = getScores(ocs); - std::vector partition = getPartition(scores,2,5,2); - - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - if(scores[i][j] >= 0){printf(" ");} - printf("%5.5f ",0.00001*scores[i][j]); - } - printf("\n"); - } - printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); + model->relativeposes = mfr.poses; @@ -720,29 +646,14 @@ void ModelUpdater::makeInitialSetup(){ cf.push_back(model->frames[i]); cm.push_back(model->modelmasks[i]); } - //getGoodCompareFrames(cp,cf,cm); - computeOcclusionAreas(cp,cf,cm); - exit(0); + getGoodCompareFrames(cp,cf,cm); + model->rep_relativeposes = cp; model->rep_frames = cf; model->rep_modelmasks = cm; - model->save("latestModel"); - // vector > ocs2 = computeOcclusionScore(model->submodels,mfr.poses,1,false); - // std::vector > scores2 = getScores(ocs2); - - // double sumscore_aft = 0; - // for(unsigned int i = 0; i < scores2.size(); i++){ - // for(unsigned int j = 0; j < scores2.size(); j++){ - // sumscore_aft += scores2[i][j]; - // } - // } - - // printf("bef %f after %f\n",sumscore_bef,sumscore_aft); - // if(sumscore_aft >= sumscore_bef){ - // model->submodels_relativeposes = mfr.poses; - // } + printf("model->rep_relativeposes: %i\n",model->rep_relativeposes.size()); } void ModelUpdater::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, int type, bool debugg){ @@ -2247,7 +2158,7 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & point.x = tx; point.y = ty; point.z = tz; - point.r = 255; + point.r = 0; point.g = 0; point.b = 0; src_cloud->points[src_ind] = point; @@ -2272,7 +2183,7 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & point.x = dst_x; point.y = dst_y; point.z = dst_z; - point.r = 0; + point.r = 255; point.g = 0; point.b = 255; dst_cloud->points[dst_ind] = point; @@ -2281,6 +2192,9 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & } } + int testsum = 0; + + for(unsigned int src_w = 0; src_w < src_width;src_w++){ for(unsigned int src_h = 0; src_h < src_height;src_h++){ int src_ind = src_h*src_width+src_w; @@ -2344,12 +2258,36 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec.push_back(d); nvec.push_back(1-surface_angle); }else{ - double noise = 0.02; - double noise_angle = 0.025; - double p_overlap = exp(-0.5*(d*d)/(noise*noise)); - double p_overlap_angle = exp(-0.5*((1-surface_angle)*(1-surface_angle))/(noise_angle*noise_angle)); - p_overlap = std::min(0.9,p_overlap); - double p_occlusion = (1.0-p_overlap)*normalCFD(d/noise); + + + +// double noise = 0.02; +// double noise_angle = 0.05; +// double p_overlap = exp(-0.5*(d*d)/(noise*noise)); +// double p_overlap_angle = exp(-0.5*((1-surface_angle)*(1-surface_angle))/(noise_angle*noise_angle)); +// p_overlap = std::min(0.9,p_overlap); +// double p_occlusion = (1.0-p_overlap)*normalCFD(d/noise); +// p_overlap *= p_overlap_angle; + +// if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0){ +// interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); +// interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap)); +// } +// if(debugg){ +// src_cloud->points[src_ind].r = 255-255.0 * func_overlap_angle; +// src_cloud->points[src_ind].g = 255.0 * func_overlap_angle; +// src_cloud->points[src_ind].b = 0; +// } +// if(dst_detdata[dst_ind] != 0){continue;} +// if(src_detdata[src_ind] != 0){continue;} + +// overlaps[src_ind] = std::max(overlaps[src_ind],p_overlap); +// occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); + + double p_overlap_angle = nfunc->getProb(1-surface_angle); + double p_overlap = dfunc->getProb(d); + double p_occlusion = dfunc->getProbInfront(d); + p_overlap *= p_overlap_angle; if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0){ @@ -2357,12 +2295,22 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap)); } + if(debugg){ + src_cloud->points[src_ind].r = 255.0 * p_overlap; + src_cloud->points[src_ind].g = 255.0 * p_occlusion; + src_cloud->points[src_ind].b = 0; + } + +// if(debugg){ +// src_cloud->points[src_ind].r = 255-255.0 * func_overlap_angle; +// src_cloud->points[src_ind].g = 255.0 * func_overlap_angle; +// src_cloud->points[src_ind].b = 0; +// } if(dst_detdata[dst_ind] != 0){continue;} if(src_detdata[src_ind] != 0){continue;} overlaps[src_ind] = std::max(overlaps[src_ind],p_overlap); occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); - } // if(fabs(d) < 0.02){//If close, according noises, and angle of the surfaces similar: FUSE // if(surface_angle > 0.9){ @@ -2387,6 +2335,7 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & } if(debugg){ + printf("testsum: %i\n",testsum); viewer->removeAllPointClouds(); viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); @@ -2415,6 +2364,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s int current_point = 0; float * priors = new float[3*tot_nr_pixels]; + bool * valids = new bool[tot_nr_pixels]; //bool store_distance = true; @@ -2423,118 +2373,69 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s DistanceWeightFunction2 * dfunc; DistanceWeightFunction2 * nfunc; -// for(unsigned int i = 0; i < frames.size(); i++){ -// for(unsigned int j = 0; j < frames.size(); j++){ -// if(i == j){continue;} -// Eigen::Matrix4d p = poses[i].inverse() * poses[j]; -// getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i], 0, 0, frames[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); -// } + for(unsigned int i = 0; i < frames.size(); i++){ + for(unsigned int j = 0; j < frames.size(); j++){ + if(i == j){continue;} + Eigen::Matrix4d p = poses[i].inverse() * poses[j]; + getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i], 0, 0, frames[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); + } -// for(unsigned int j = 0; j < bgcf.size(); j++){ -// Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; -// getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i], 0, 0, bgcf[j],-1,-1,interframe_connectionId,interframe_connectionStrength,false); -// } -// } - -// double dstdval = 0; -// for(unsigned int i = 0; i < dvec.size(); i++){ -// dstdval += dvec[i]*dvec[i]; -// } -// dstdval = sqrt(dstdval/double(dvec.size()-1)); - - -// DistanceWeightFunction2PPR2 * dfuncTMP = new DistanceWeightFunction2PPR2(); -// dfunc = dfuncTMP; -// dfuncTMP->refine_mean = true; -// dfuncTMP->zeromean = false; -// dfuncTMP->startreg = 0.0001; -// dfuncTMP->debugg_print = true; -// dfuncTMP->bidir = true; -// dfuncTMP->maxp = 0.999999; -// dfuncTMP->maxd = 0.03; -// dfuncTMP->histogram_size = 1000; -// dfuncTMP->fixed_histogram_size = true; -// dfuncTMP->startmaxd = dfuncTMP->maxd; -// dfuncTMP->starthistogram_size = dfuncTMP->histogram_size; -// dfuncTMP->blurval = 0.5; -// dfuncTMP->stdval2 = dstdval; -// dfuncTMP->maxnoise = dstdval; -// dfuncTMP->reset(); -// dfunc->computeModel(dvec); - - -// DistanceWeightFunction2PPR2 * nfuncTMP = new DistanceWeightFunction2PPR2(); -// nfunc = nfuncTMP; -// nfuncTMP->startreg = 0.0001; -// nfuncTMP->debugg_print = true; -// nfuncTMP->bidir = true; -// nfuncTMP->zeromean = false; -// nfuncTMP->maxp = 0.999999; -// nfuncTMP->maxd = 1.0; -// nfuncTMP->histogram_size = 1000; -// nfuncTMP->fixed_histogram_size = true; -// nfuncTMP->startmaxd = nfuncTMP->maxd; -// nfuncTMP->starthistogram_size = nfuncTMP->histogram_size; -// nfuncTMP->blurval = 0.5; -// nfuncTMP->stdval2 = 1; -// nfuncTMP->maxnoise = 1; -// nfuncTMP->reset(); -// nfunc->computeModel(nvec); - -//// for(unsigned int w = 1; w < width-1;w++){ -//// for(unsigned int h = 1; h < height-1;h++){ -//// int ind = h*width+w; -//// float z = idepth*float(depthdata[ind]); - -//// if(w > 1){ -//// int dir = -1; -//// int other2 = ind+2*dir; -//// int other = ind+dir; - -//// float z3 = idepth*float(depthdata[other2]); -//// float z2 = idepth*float(depthdata[other]); -//// z2 = 2*z2-z3; - -//// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} -//// } - -//// if(h > 1){ -//// int dir = -width; -//// int other2 = ind+2*dir; -//// int other = ind+dir; - -//// float z3 = idepth*float(depthdata[other2]); -//// float z2 = idepth*float(depthdata[other]); -//// z2 = 2*z2-z3; - -//// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} -//// } -//// } -//// } - -//// std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} -//// std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} - -//// for(unsigned int w = 1; w < width-1;w++){ -//// for(unsigned int h = 1; h < height-1;h++){ -//// int ind = h*width+w; -//// float z = idepth*float(depthdata[ind]); - -//// if(w > 1){ -//// int dir = -1; -//// int other2 = ind+2*dir; -//// int other = ind+dir; - - -//// float z3 = idepth*float(depthdata[other2]); -//// float z2 = idepth*float(depthdata[other]); - -//// float dz = fabs(z-z2); -//// if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} -//// if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} -//delete dfuncTMP; - -//exit(0); + for(unsigned int j = 0; j < bgcf.size(); j++){ + //printf("%i , %i\n",i,j); + //Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; + //getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i], 0, 0, bgcf[j],-1,-1,interframe_connectionId,interframe_connectionStrength,debugg); + } + } + + + + double dstdval = 0; + for(unsigned int i = 0; i < dvec.size(); i++){ + dstdval += dvec[i]*dvec[i]; + } + dstdval = sqrt(dstdval/double(dvec.size()-1)); + + + DistanceWeightFunction2PPR2 * dfuncTMP = new DistanceWeightFunction2PPR2(); + dfunc = dfuncTMP; + dfuncTMP->refine_mean = true; + dfuncTMP->zeromean = false; + dfuncTMP->startreg = 0.0005; + dfuncTMP->debugg_print = false; + dfuncTMP->bidir = true; + dfuncTMP->maxp = 0.9; + dfuncTMP->maxd = 0.03; + dfuncTMP->histogram_size = 1000; + dfuncTMP->fixed_histogram_size = true; + dfuncTMP->max_under_mean = false; + dfuncTMP->startmaxd = dfuncTMP->maxd; + dfuncTMP->starthistogram_size = dfuncTMP->histogram_size; + dfuncTMP->blurval = 0.5; + dfuncTMP->stdval2 = dstdval; + dfuncTMP->maxnoise = dstdval; + dfuncTMP->compute_infront = true; + dfuncTMP->reset(); + dfunc->computeModel(dvec); + + + DistanceWeightFunction2PPR2 * nfuncTMP = new DistanceWeightFunction2PPR2(); + nfunc = nfuncTMP; + nfuncTMP->startreg = 0.01; + nfuncTMP->debugg_print = false; + nfuncTMP->bidir = false; + nfuncTMP->zeromean = true; + nfuncTMP->maxp = 0.999999; + nfuncTMP->maxd = 1.0; + nfuncTMP->histogram_size = 1000; + nfuncTMP->fixed_histogram_size = true; + nfuncTMP->startmaxd = nfuncTMP->maxd; + nfuncTMP->starthistogram_size = nfuncTMP->histogram_size; + nfuncTMP->blurval = 0.5; + nfuncTMP->stdval2 = 1; + nfuncTMP->maxnoise = 1; + nfuncTMP->ggd = true; + nfuncTMP->reset(); + nfunc->computeModel(nvec); int frameConnections = 0; @@ -2586,11 +2487,16 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i], bg_overlaps, bg_occlusions, bgcf[j],-1,-1,interframe_connectionId,interframe_connectionStrength,false); } + + unsigned char * detdata = (unsigned char*)(frame->det_dilate.data); float minprob = 0.01; for(unsigned int h = 0; h < height;h++){ for(unsigned int w = 0; w < width;w++){ int ind = h*width+w; + + valids[offset+ind] = detdata[ind] == 0; + float p_moving = current_occlusions[ind]; float p_dynamic = std::max((bg_occlusions[ind]-p_moving)*current_overlaps[ind],0.0); p_dynamic += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); @@ -2602,18 +2508,33 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s float notMoving = current_overlaps[ind]; float bias = 0.0001; + + + float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; - float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover) + minprob; - float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover) + minprob; - float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover) + 2.0*minprob; + float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover); + float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover); + float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover); priors[3*(offset+ind)+0] = p_moving_tot; priors[3*(offset+ind)+1] = p_dynamic_tot; priors[3*(offset+ind)+2] = p_static_tot; +// float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; +// float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; +// float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; + +// float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover) + minprob; +// float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover) + minprob; +// float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover) + 2.0*minprob; + +// priors[3*(offset+ind)+0] = p_moving_tot; +// priors[3*(offset+ind)+1] = p_dynamic_tot; +// priors[3*(offset+ind)+2] = p_static_tot; + if(probs[0][ind] > 0.00000001){frameConnections++;} if(probs[1][ind] > 0.00000001){frameConnections++;} @@ -2648,6 +2569,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s delete[] bg_overlaps; } + delete dfuncTMP; + delete nfuncTMP; + long interframeConnections = 0; for(unsigned int i = 0; i < interframe_connectionId.size();i++){ for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ @@ -2664,6 +2588,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s g -> add_node(); double p_fg = priors[3*i+0]+priors[3*i+1]; double p_bg = priors[3*i+2]; + double norm = p_fg + p_bg; + p_fg /= norm; + p_bg /= norm; if(priors[3*i+0]+priors[3*i+1]+priors[3*i+2] <= 0){ g -> add_tweights( i, 0, 0 ); continue; @@ -2907,37 +2834,72 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for (unsigned int d = 0; d < dynamic_indices.size(); d++){ double score = 0; pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud_cluster2 (new pcl::PointCloud); double sum0 = 0; double sum1 = 0; double sum2 = 0; + double totsum = 0; for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ int pid = dynamic_indices[d].indices[ind]; int dind = dynamicdata[pid]; - score += priors[3*dind+1] -0.5*(priors[3*dind+0]+priors[3*dind+2]); - sum0 += priors[3*dind+0]; - sum1 += priors[3*dind+1]; - sum2 += priors[3*dind+2]; + double p0 = priors[3*dind+0]; + double p1 = priors[3*dind+1]; + double p2 = priors[3*dind+2]; + + double s = 0; + if(valids[dind]){ + if(p0 > p2){ + s += p1 - p0; + }else{ + s += p1 - p2; + } + score += s; + + sum0 += priors[3*dind+0]; + sum1 += priors[3*dind+1]; + sum2 += priors[3*dind+2]; + totsum++; + } + + + + + + + pcl::PointXYZRGBNormal p = cloud->points[dind]; + + p.r = 0; + p.g = 0; + p.b = 0; + + if(s > 0){p.g = 255.0*s;} + if(s < 0){p.r = -255.0*s;} cloud_cluster->points.push_back(dynamiccloud->points[pid]); + cloud_cluster2->points.push_back(p); } - double avg = score/double(dynamic_indices[d].indices.size()); + double avg = score/totsum; -// printf("score: %f avg: %f\n",score,avg); -// printf("sum0: %f sum1: %f sum2: %f\n",sum0,sum1,sum2); - if(score > 100 && avg > 0.075){ + printf("score: %f avg: %f\n",score,avg); + printf("sum0: %f sum1: %f sum2: %f\n",sum0,sum1,sum2); + if(score > 100){ for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ labels[dynamicdata[dynamic_indices[d].indices[ind]]] = 3; } } -// if(cloud_cluster->points.size() > 500){ -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); -// viewer->spin(); -// } + if(cloud_cluster->points.size() > 500){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); + viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_cluster2, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster2), "cloud_cluster"); + viewer->spin(); + } } start_inf = getTime(); @@ -2977,7 +2939,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s double avg = score/double(moving_indices[d].indices.size()); // printf("score: %f avg: %f\n",score,avg); // printf("sum0: %f sum1: %f sum2: %f\n",sum0,sum1,sum2); - if(score > 100 && avg > 0.075){ + if(score > 100){ for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ labels[movingdata[moving_indices[d].indices[ind]]] = 4; } @@ -3014,14 +2976,23 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } if(debugg){ + pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); + printf("3-class\n"); for(unsigned int i = 0; i < current_point; i++){ cloud->points[i].r = priors[3*i+0]*255.0; cloud->points[i].g = priors[3*i+1]*255.0; cloud->points[i].b = priors[3*i+2]*255.0; } + + cloud_sample->points.clear(); + for(unsigned int i = 0; i < current_point; i++){ + if(rand() % 4 == 0){ + cloud_sample->points.push_back(cloud->points[i]); + } + } viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); viewer->spin(); printf("results-class\n"); @@ -3056,8 +3027,17 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s cloud->points[i].b = 0; } } + + + + cloud_sample->points.clear(); + for(unsigned int i = 0; i < current_point; i++){ + if(rand() % 4 == 0){ + cloud_sample->points.push_back(cloud->points[i]); + } + } viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); viewer->spin(); } diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp index 5e9b47e6..e6bb66b5 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp @@ -68,12 +68,18 @@ VectorXd DistanceWeightFunction2::getProbs(MatrixXd mat){ return W;//VectorXf(mat.rows()); } -double DistanceWeightFunction2::getProb(double d){ +double DistanceWeightFunction2::getProb(double d, bool debugg){ printf("double DistanceWeightFunction2::getProbs(double d){ not implemented\n"); exit(0); return 0; } +double DistanceWeightFunction2::getProbInfront(double d, bool debugg){ + printf("double DistanceWeightFunction2::getProbInfront(double d){ not implemented\n"); + exit(0); + return 0; +} + double DistanceWeightFunction2::getNoise(){return 1;} bool DistanceWeightFunction2::update(){return true;} void DistanceWeightFunction2::reset(){} diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp index 49defbcc..8361c15a 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp @@ -16,13 +16,17 @@ DistanceWeightFunction2PPR2::DistanceWeightFunction2PPR2( double maxd_, int hist maxnoise = 99999999999999; noiseval = 100.0; - + prob.resize(histogram_size+1); prob[histogram_size] = 0; + + infront.resize(histogram_size+1); + infront[histogram_size] = 1; histogram.resize(histogram_size+1); blur_histogram.resize(histogram_size+1); noise.resize(histogram_size+1); + noisecdf.resize(histogram_size+1); refine_mean = false; refine_mul = false; @@ -83,7 +87,11 @@ DistanceWeightFunction2PPR2::DistanceWeightFunction2PPR2( double maxd_, int hist refine_std = true; nr_refineiters = 1; convergence_threshold = 0.05; + + ggd = false; + compute_infront = false; } + DistanceWeightFunction2PPR2::~DistanceWeightFunction2PPR2(){} class Gaussian3 { @@ -105,6 +113,11 @@ class Gaussian3 { double dx = mean-x; return mul*exp(dx*dx*scaledinformation); } + + double getcdf(double x) + { + return 0.5 * erfc(-((x-mean)/stdval) * M_SQRT1_2); + } }; class gaussian2 { @@ -685,26 +698,20 @@ if(!fixed_histogram_size){ } } - const double histogram_mul = double(histogram_size)/maxd; + histogram_mul = double(histogram_size)/maxd; + if(bidir){ + histogram_mul2 = double(histogram_size)/(2.0*maxd); + }else{ + histogram_mul2 = double(histogram_size)/maxd; + } if(debugg_print){printf("histogram_mul: %f histogram_size: %f maxd: %f\n",histogram_mul,double(histogram_size),maxd);} double start_time = getCurrentTime3(); for(int j = 0; j < histogram_size; j++){histogram[j] = 0;} -// for(unsigned int j = 0; j < nr_data; j++){ -// for(int k = 0; k < nr_dim; k++){ -// double ind = fabs(mat(k,j))*histogram_mul; -// double w2 = ind-int(ind); -// double w1 = 1-w2; -// if(ind >= 0 && (ind+0.5) < histogram_size){ -// histogram[int(ind+0.5)]++; -// } -// if(j % 5000 == 0){printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind);} -// } -// } -if(!fixed_histogram_size){ + if(!fixed_histogram_size){ for(unsigned int j = 0; j < nr_data; j++){ for(int k = 0; k < nr_dim; k++){ double ind = fabs(mat(k,j))*histogram_mul; @@ -714,49 +721,36 @@ if(!fixed_histogram_size){ } } histogram[0]*=2; -}else{ +}else if(bidir){ + for(unsigned int j = 0; j < histogram_size; j++){histogram[j] = 0;} for(unsigned int j = 0; j < nr_data; j++){ for(int k = 0; k < nr_dim; k++){ - double ind = fabs(mat(k,j))*histogram_mul; - double w2 = ind-int(ind); - double w1 = 1-w2; + double ind = double(histogram_size)*(mat(k,j)+maxd)/(2.0*maxd); if(ind >= 0 && (ind+0.00001) < histogram_size){ histogram[int(ind+0.00001)]++; } - //if(j % 5000 == 0){printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind);} } } -} - -if(bidir){ - printf("%i\n",__LINE__); - for(unsigned int j = 0; j < histogram_size; j++){histogram[j] = 0;} +}else{ for(unsigned int j = 0; j < nr_data; j++){ for(int k = 0; k < nr_dim; k++){ - double ind = double(histogram_size)*(mat(k,j)+maxd)/(2.0*maxd); + double ind = fabs(mat(k,j))*histogram_mul; + double w2 = ind-int(ind); + double w1 = 1-w2; if(ind >= 0 && (ind+0.00001) < histogram_size){ histogram[int(ind+0.00001)]++; } + //if(j % 5000 == 0){printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind);} } } - - //if(debugg_print){ - //} } + start_time = getCurrentTime3(); blurHistogram3(blur_histogram,histogram,blurval,histogram_size,debugg_print); - //blurHistogram2(blur_histogram,histogram,blurval,false); -//if(bidir){ -// getGGModel(blur_histogram,histogram_size); -// exit(0); -//} - - Gaussian3 g = getModel(stdval,blur_histogram,uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); - - //printf("found stdval: %f\n",stdval); stdval2 = 0; - Gaussian3 g2 = getModel(stdval2,histogram,uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); + Gaussian3 g = getModel(stdval, blur_histogram, uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); + Gaussian3 g2 = getModel(stdval2,histogram, uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); stdval2 = std::max(1.0,stdval2); stdval2 = maxd*stdval2/float(histogram_size); @@ -789,7 +783,10 @@ if(bidir){ g.stdval += histogram_size*regularization/maxd; g.update(); - for(int k = 0; k < histogram_size; k++){ noise[k] = g.getval(k);} + for(int k = 0; k < histogram_size; k++){noise[k] = g.getval(k);} + if(compute_infront){ + for(int k = 0; k < histogram_size; k++){noisecdf[k] = g.getcdf(k);} + } std::vector new_histogram; if(rescaling){ @@ -814,6 +811,7 @@ if(bidir){ double hs = blur_histogram[k]+1; if(rescaling){hs = new_histogram[k]+1;} prob[k] = std::min(maxp , noise[k]/hs);//never fully trust any data + infront[k] = (1-prob[k])*noisecdf[k]; } } @@ -833,45 +831,28 @@ if(bidir){ } - - if(bidir){ - if(debugg_print){printf("mul: %10.10f mean: %10.10f stdval: %10.10f\n",g.mul,g.mean,g.stdval);} - if(debugg_print){printf("hist = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){ printf("%i ",int(histogram[k]));} printf("];\n");} - if(debugg_print){printf("noise = ["); for(int k = 0; k < 3000 && k < int(noise.size()); k++){ printf("%i ",int(noise[k]));} printf("];\n");} - if(debugg_print){printf("hist_smooth = ["); for(int k = 0; k < 3000 && k < int(blur_histogram.size()); k++){ printf("%i ",int(blur_histogram[k]));} printf("];\n");} - if(debugg_print){printf("clf; hold on; plot(hist,'r'); plot(noise,'b'); plot(hist_smooth,'g');\n");} - exit(0); - } - - - if(debugg_print){printf("hist = ["); for(int k = 0; k < 300 && k < histogram_size; k++){ printf("%i ",int(histogram[k]));} printf("];\n");} - if(debugg_print){printf("noise = ["); for(int k = 0; k < 300 && k < int(noise.size()); k++){ printf("%i ",int(noise[k]));} printf("];\n");} - if(debugg_print){printf("hist_smooth = ["); for(int k = 0; k < 300 && k < int(blur_histogram.size()); k++){ printf("%i ",int(blur_histogram[k]));} printf("];\n");} - //if(debugg_print){printf("new_histogram = ["); for(int k = 0; k < 300 && k < int(new_histogram.size()); k++){ printf("%i ",int(new_histogram[k]));} printf("];\n");} - if(debugg_print){printf("clf; hold on; plot(hist,'r'); plot(noise,'b'); plot(hist_smooth,'g');\n");} if(false){printf("meanoffset: %f stdval2: %f stdval: %f regularization: %f\n",meanval2,stdval2,noiseval,regularization);} if(!fixed_histogram_size && update_size ){ double next_maxd = meanval2 + (stdval2 + regularization)*target_length; - //printf("mean %f stdval %f regularization: %f\n",meanval2,stdval2,regularization); double logdiff = log(next_maxd/maxd); - //if(debugg_print){printf("maxd: %f next_maxd: %f logdiff: %f \n",maxd,next_maxd,logdiff);} - //if(debugg_print){printf("meanoffset: %f stdval2: %f stdval: %f regularization: %f\n",meanval2,stdval2,noiseval,regularization);} - // if(fabs(logdiff) > 0.2 && iter < 30){ iter++; computeModel(mat); return; }else{iter = 0;} } - if(debugg_print){printf("prob = ["); for(int k = 0; k < 300 && k < histogram_size; k++){printf("%2.2f ",prob[k]);} printf("];\n");} - - //debugg_print = false; + if(debugg_print){printf("hist = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(histogram[k]));} printf("];\n");} + if(debugg_print){printf("noise = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(noise[k]));} printf("];\n");} + if(debugg_print){printf("noisecdf = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",int(noisecdf[k]));} printf("];\n");} + if(debugg_print){printf("hist_smooth = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(blur_histogram[k]));} printf("];\n");} + if(debugg_print){printf("prob = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",prob[k]);} printf("];\n");} + if(debugg_print){printf("infront = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",infront[k]);} printf("];\n");} + if(debugg_print){printf("clf; hold on; plot(hist_smooth,'r'); plot(noise,'b'); plot(prob*max(noise),'g');\n");} } VectorXd DistanceWeightFunction2PPR2::getProbs(MatrixXd mat){ const unsigned int nr_data = mat.cols(); const int nr_dim = mat.rows(); - const float histogram_mul = float(histogram_size)/maxd; nr_inliers = 0; VectorXd weights = VectorXd(nr_data); @@ -879,10 +860,18 @@ VectorXd DistanceWeightFunction2PPR2::getProbs(MatrixXd mat){ float inl = 1; float ninl = 1; for(int k = 0; k < nr_dim; k++){ - double ind = fabs(mat(k,j))*histogram_mul; + double ind; + float p = 0; + if(bidir){ + ind = double(histogram_size)*(mat(k,j)+maxd)/(2.0*maxd); + }else{ + ind = fabs(mat(k,j))*histogram_mul; + } + + double w2 = ind-int(ind); double w1 = 1-w2; - float p = 0; + if(ind >= 0 && (ind+0.5) < histogram_size){ if(interp){ p = prob[int(ind)]*w1 + prob[int(ind+1)]*w2; }else{ p = prob[int(ind+0.5)];} @@ -901,10 +890,65 @@ VectorXd DistanceWeightFunction2PPR2::getProbs(MatrixXd mat){ return weights; } -double DistanceWeightFunction2PPR2::getProb(double d){ - const float histogram_mul = float(histogram_size)/maxd; +double DistanceWeightFunction2PPR2::getProb(double d, bool debugg){ + double ind = getInd(d,debugg); + float p = 0; + if(interp){ + double w2 = ind-int(ind); + double w1 = 1-w2; + if(ind >= 0 && (ind+1) < histogram_size){ + p = prob[int(ind)]*w1 + prob[int(ind+1)]*w2; + } + }else{ + if(ind >= 0 && ind < histogram_size){ + p = prob[int(ind)]; + } + } + + + if(debugg){printf("d: %5.5f -> ind: %5.5f -> p: %5.5f histogramsize: %5.5i maxd: %5.5f\n",d,ind,p,histogram_size,maxd);} + return p; + +/* + * //const float histogram_mul = float(histogram_size)/maxd; double ind = fabs(d)*histogram_mul; + double w2 = ind-int(ind); + double w1 = 1-w2; + float p = 0; + if(ind >= 0 && (ind+0.5) < histogram_size){ + if(interp){ p = prob[int(ind)]*w1 + prob[int(ind+1)]*w2; + }else{ p = prob[int(ind+0.5)];} + } + return p; + */ +} + +double DistanceWeightFunction2PPR2::getProbInfront(double d, bool debugg){ + double ind = getInd(d,debugg); + float p; + if(ind > meanval){ p = 1;} + else{ p = 0;} + + if(interp){ + double w2 = ind-int(ind); + double w1 = 1-w2; + if(ind >= 0 && (ind+1) < histogram_size){ + p = infront[int(ind)]*w1 + infront[int(ind+1)]*w2; + } + }else{ + if(ind >= 0 && ind < histogram_size){ + p = infront[int(ind)]; + } + } + + + if(debugg){printf("d: %5.5f -> ind: %5.5f -> infront: %5.5f histogramsize: %5.5i maxd: %5.5f\n",d,ind,p,histogram_size,maxd);} + return p; + +/* + * //const float histogram_mul = float(histogram_size)/maxd; + double ind = fabs(d)*histogram_mul; double w2 = ind-int(ind); double w1 = 1-w2; float p = 0; @@ -913,6 +957,7 @@ double DistanceWeightFunction2PPR2::getProb(double d){ }else{ p = prob[int(ind+0.5)];} } return p; + */ } bool DistanceWeightFunction2PPR2::update(){ @@ -968,11 +1013,6 @@ void DistanceWeightFunction2PPR2::reset(){ iter = 0; first = true; -// if(debugg_print){ -// printf("starthistogram_size: %i startmaxd: %f\n",starthistogram_size,startmaxd); -// printf("histogram_size: %i maxd: %f\n",histogram_size,maxd); -// exit(0); -// } } std::string DistanceWeightFunction2PPR2::getString(){ @@ -990,6 +1030,15 @@ double DistanceWeightFunction2PPR2::getConvergenceThreshold(){ return convergence_threshold; } } + +inline double DistanceWeightFunction2PPR2::getInd(double d, bool debugg){ + if(bidir){ + + return 0.00001+(d+maxd)*histogram_mul2; + }else{ + return 0.00001+fabs(d)*histogram_mul2; + } +} } diff --git a/strands_sweep_registration/src/RobotContainer.cpp b/strands_sweep_registration/src/RobotContainer.cpp index 5a6b5080..b8872eb5 100644 --- a/strands_sweep_registration/src/RobotContainer.cpp +++ b/strands_sweep_registration/src/RobotContainer.cpp @@ -118,10 +118,10 @@ RobotContainer::~RobotContainer(){ void RobotContainer::initializeCamera(double fx, double fy, double cx, double cy, unsigned int w, unsigned int h) { - fx = 533.796412;//535; - fy = 533.112736;//fx; - cx = 314.863333;//double(w-1.0)/2.0; - cy = 241.271340;//double(h-1.0)/2.0; +// fx = fx;//533.796412;//535; +// fy = 533.112736;//fx; +// cx = 314.863333;//double(w-1.0)/2.0; +// cy = 241.271340;//double(h-1.0)/2.0; std::cout<<"Initializing camera with parameters "< Date: Mon, 5 Sep 2016 09:53:27 +0200 Subject: [PATCH 085/255] ... --- .../src/modelupdater/ModelUpdater.cpp | 193 ++++++++++-------- 1 file changed, 113 insertions(+), 80 deletions(-) diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 6fc5f7bb..ac483891 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2519,9 +2519,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover); float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover); - priors[3*(offset+ind)+0] = p_moving_tot; - priors[3*(offset+ind)+1] = p_dynamic_tot; - priors[3*(offset+ind)+2] = p_static_tot; + priors[3*(offset+ind)+0] = p_moving_tot; + priors[3*(offset+ind)+1] = p_dynamic_tot; + priors[3*(offset+ind)+2] = p_static_tot; // float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; // float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; @@ -2556,9 +2556,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s point.x = m00*x + m01*y + m02*z + m03; point.y = m10*x + m11*y + m12*z + m13; point.z = m20*x + m21*y + m22*z + m23; - point.r = priors[3*(offset+ind)+0]*255.0; - point.g = priors[3*(offset+ind)+1]*255.0; - point.b = priors[3*(offset+ind)+2]*255.0; + point.r = priors[3*(offset+ind)+0]*255.0; + point.g = priors[3*(offset+ind)+1]*255.0; + point.b = priors[3*(offset+ind)+2]*255.0; cloud->points.push_back(point); } } @@ -2586,12 +2586,12 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s gc::Graph * g = new gc::Graph(current_point,frameConnections+interframeConnections); for(unsigned long i = 0; i < current_point;i++){ g -> add_node(); - double p_fg = priors[3*i+0]+priors[3*i+1]; - double p_bg = priors[3*i+2]; + double p_fg = priors[3*i+0]+priors[3*i+1]; + double p_bg = priors[3*i+2]; double norm = p_fg + p_bg; p_fg /= norm; p_bg /= norm; - if(priors[3*i+0]+priors[3*i+1]+priors[3*i+2] <= 0){ + if(priors[3*i+0]+priors[3*i+1]+priors[3*i+2] <= 0){ g -> add_tweights( i, 0, 0 ); continue; } @@ -2702,8 +2702,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s gc::Graph * dynamic_g = new gc::Graph(nr_dynamic,dynamic_frameConnections+dynamic_interframeConnections); for(unsigned long i = 0; i < dyn_ind.size();i++){ dynamic_g -> add_node(); - double p_fg = priors[3*dyn_ind[i]+0]; - double p_bg = priors[3*dyn_ind[i]+1]; + double p_fg = priors[3*dyn_ind[i]+0]; + double p_bg = priors[3*dyn_ind[i]+1]; double norm = p_fg+p_bg; p_fg /= norm; p_bg /= norm; @@ -2832,47 +2832,29 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s end_inf = getTime(); printf("dynamic cluster time: %10.10fs\n",end_inf-start_inf); for (unsigned int d = 0; d < dynamic_indices.size(); d++){ - double score = 0; + double score = 0; pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); pcl::PointCloud::Ptr cloud_cluster2 (new pcl::PointCloud); - double sum0 = 0; - double sum1 = 0; - double sum2 = 0; - double totsum = 0; + + double totsum = 0; for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ int pid = dynamic_indices[d].indices[ind]; int dind = dynamicdata[pid]; - double p0 = priors[3*dind+0]; - double p1 = priors[3*dind+1]; - double p2 = priors[3*dind+2]; + double p0 = priors[3*dind+0]; + double p1 = priors[3*dind+1]; + double p2 = priors[3*dind+2]; double s = 0; if(valids[dind]){ - if(p0 > p2){ - s += p1 - p0; - }else{ - s += p1 - p2; - } - score += s; - - sum0 += priors[3*dind+0]; - sum1 += priors[3*dind+1]; - sum2 += priors[3*dind+2]; + if(p0 > p2){s += p1 - p0;} + else{ s += p1 - p2;} + score += s; totsum++; } - - - - - - pcl::PointXYZRGBNormal p = cloud->points[dind]; - - p.r = 0; - p.g = 0; - p.b = 0; - + pcl::PointXYZRGBNormal p = cloud->points[dind]; + p.r = 0;p.g = 0;p.b = 0; if(s > 0){p.g = 255.0*s;} if(s < 0){p.r = -255.0*s;} @@ -2880,21 +2862,17 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s cloud_cluster2->points.push_back(p); } - double avg = score/totsum; - - printf("score: %f avg: %f\n",score,avg); - printf("sum0: %f sum1: %f sum2: %f\n",sum0,sum1,sum2); - if(score > 100){ +// printf("score: %f avg: %f\n",score,score/totsum); + if(score > 200){ for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ labels[dynamicdata[dynamic_indices[d].indices[ind]]] = 3; } } - - if(cloud_cluster->points.size() > 500){ + if(false && cloud_cluster->points.size() > 500){ viewer->removeAllPointClouds(); viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); - viewer->spin(); + viewer->spin(); viewer->removeAllPointClouds(); viewer->addPointCloud (cloud_cluster2, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster2), "cloud_cluster"); @@ -2918,38 +2896,93 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s end_inf = getTime(); printf("moving cluster time: %10.10fs\n",end_inf-start_inf); - for (unsigned int d = 0; d < moving_indices.size(); d++){ - double score = 0; - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - double sum0 = 0; - double sum1 = 0; - double sum2 = 0; - for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ - int pid = moving_indices[d].indices[ind]; - int dind = movingdata[pid]; - score += priors[3*dind+0] -0.5*(priors[3*dind+1]+priors[3*dind+2]); - - sum0 += priors[3*dind+0]; - sum1 += priors[3*dind+1]; - sum2 += priors[3*dind+2]; - - cloud_cluster->points.push_back(movingcloud->points[pid]); - } - - double avg = score/double(moving_indices[d].indices.size()); -// printf("score: %f avg: %f\n",score,avg); -// printf("sum0: %f sum1: %f sum2: %f\n",sum0,sum1,sum2); - if(score > 100){ - for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ - labels[movingdata[moving_indices[d].indices[ind]]] = 4; - } - } - -// if(cloud_cluster->points.size() > 500){ -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); -// viewer->spin(); -// } + for (unsigned int d = 0; d < moving_indices.size(); d++){ + double score = 0; + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud_cluster2 (new pcl::PointCloud); + + double totsum = 0; + double sum0 = 0; + double sum1 = 0; + double sum2 = 0; + for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ + int pid = moving_indices[d].indices[ind]; + int dind = movingdata[pid]; + + double p0 = priors[3*dind+0]; + double p1 = priors[3*dind+1]; + double p2 = priors[3*dind+2]; + + double s = 0; + if(valids[dind]){ + if(p1 > p2){s += p0 - p1;} + else{ s += p0 - p2;} + score += s; + totsum++; + sum0 += p0; + sum1 += p1; + sum2 += p2; + } + + pcl::PointXYZRGBNormal p = cloud->points[dind]; + p.r = 0;p.g = 0;p.b = 0; + if(s > 0){p.g = 255.0*s;} + if(s < 0){p.r = -255.0*s;} + + cloud_cluster->points.push_back(movingcloud->points[pid]); + cloud_cluster2->points.push_back(p); + } + +// printf("score: %f avg: %f\n",score,score/totsum); +// printf("sum: %f %f %f\n",sum0,sum1,sum2); + if(score > 200){ + for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ + labels[movingdata[moving_indices[d].indices[ind]]] = 4; + } + } + + if(false && cloud_cluster->points.size() > 500){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); + viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_cluster2, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster2), "cloud_cluster"); + viewer->spin(); + } + + +// double score = 0; +// pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); +// double sum0 = 0; +// double sum1 = 0; +// double sum2 = 0; +// for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ +// int pid = moving_indices[d].indices[ind]; +// int dind = movingdata[pid]; +// score += priors[3*dind+0] -0.5*(priors[3*dind+1]+priors[3*dind+2]); + +// sum0 += priors[3*dind+0]; +// sum1 += priors[3*dind+1]; +// sum2 += priors[3*dind+2]; + +// cloud_cluster->points.push_back(movingcloud->points[pid]); +// } + +// double avg = score/double(moving_indices[d].indices.size()); +//// printf("score: %f avg: %f\n",score,avg); +//// printf("sum0: %f sum1: %f sum2: %f\n",sum0,sum1,sum2); +// if(score > 100){ +// for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ +// labels[movingdata[moving_indices[d].indices[ind]]] = 4; +// } +// } + +//// if(cloud_cluster->points.size() > 500){ +//// viewer->removeAllPointClouds(); +//// viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); +//// viewer->spin(); +//// } } From ec318bf433eb622ede8d721f47262312b4edd7dc Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 5 Sep 2016 11:17:13 +0200 Subject: [PATCH 086/255] Added ability to reload vocabulary in retrieval node if it changed in the processing pipeline --- .../dynamic_object_retrieval/summary_types.h | 5 ++- .../src/quasimodo_retrieval_node.cpp | 31 +++++++++++++++++-- .../src/retrieval_vocabulary.cpp | 23 ++++++++++++++ 3 files changed, 56 insertions(+), 3 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h index 47ab651d..93624c28 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/summary_types.h @@ -85,6 +85,8 @@ struct vocabulary_summary { size_t max_training_features; size_t max_append_features; + std::string last_updated; // string for checking last update, used to know when to reload + void load(const boost::filesystem::path& data_path) { std::ifstream in((data_path / boost::filesystem::path("vocabulary_summary.json")).string()); @@ -133,7 +135,8 @@ struct vocabulary_summary { cereal::make_nvp("vocabulary_tree_size", vocabulary_tree_size), cereal::make_nvp("min_segment_features", min_segment_features), cereal::make_nvp("max_training_features", max_training_features), - cereal::make_nvp("max_append_features", max_append_features)); + cereal::make_nvp("max_append_features", max_append_features), + cereal::make_nvp("last_updated", last_updated)); } vocabulary_summary() : vocabulary_type("standard"), subsegment_type(""), noise_data_path(""), annotated_data_path(""), nbr_noise_segments(0), nbr_annotated_segments(0), diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index e87c2e94..4d5cfbbe 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -144,6 +144,21 @@ class retrieval_node { */ } + bool maybe_reload() + { + dynamic_object_retrieval::vocabulary_summary temp_summary; + temp_summary.load(vocabulary_path); + if (summary.last_updated == temp_summary.last_updated) { + return false; + } + summary = temp_summary; + vt = grouped_vocabulary_tree(); + dynamic_object_retrieval::load_vocabulary(vt, vocabulary_path); + vt.set_cache_path(vocabulary_path.string()); + vt.set_min_match_depth(3); + vt.compute_normalizing_constants(); + } + pair sweep_get_rgbd_at(const boost::filesystem::path& sweep_xml, int i) { stringstream ss; @@ -750,18 +765,30 @@ int main(int argc, char** argv) bool enable_incremental; pn.param("enable_incremental", enable_incremental, false); + ros::Rate loop_rate(10); + if (enable_incremental) { retrieval_node > rs(ros::this_node::getName()); - ros::spin(); + while (true) + { + ros::spinOnce(); + loop_rate.sleep(); + rs.maybe_reload(); + } } else { retrieval_node > rs(ros::this_node::getName()); - ros::spin(); + while (true) + { + ros::spinOnce(); + loop_rate.sleep(); + rs.maybe_reload(); + } } diff --git a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp index 0a9a0ef7..06bc456c 100644 --- a/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_vocabulary.cpp @@ -18,6 +18,11 @@ #include #include +#include // chrono::system_clock +#include // localtime +#include // stringstream +#include // put_time + /** * The most reasonable thing to keep track of which metaroom folders that we have traversed * would be to create a new json file in the vocabulary folder with a list of all the sweeps @@ -219,6 +224,11 @@ void train_vocabulary(const boost::filesystem::path& data_path) summary.nbr_annotated_segments = 0; summary.nbr_annotated_sweeps = 0; + auto now = std::chrono::system_clock::now(); + auto in_time_t = std::chrono::system_clock::to_time_t(now); + char buffer [80]; + std::strftime(buffer, 80, "%Y-%m-%d %H:%M:%S", std::localtime(&in_time_t)); + summary.last_updated = string(buffer); summary.save(vocabulary_path); uris.save(vocabulary_path); dynamic_object_retrieval::save_vocabulary(*vt, vocabulary_path); @@ -323,6 +333,14 @@ bool vocabulary_service(quasimodo_msgs::index_cloud::Request& req, quasimodo_msg resp.id = summary.nbr_noise_segments; summary.nbr_noise_segments += 1; summary.nbr_noise_sweeps++; + auto now = std::chrono::system_clock::now(); + auto in_time_t = std::chrono::system_clock::to_time_t(now); + char buffer [80]; + std::strftime(buffer, 80, "%Y-%m-%d %H:%M:%S", std::localtime(&in_time_t)); + //std::stringstream ss; + //ss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d %H:%M:%S"); + //summary.last_updated = ss.str(); + summary.last_updated = string(buffer); summary.save(vocabulary_path); dynamic_object_retrieval::save_vocabulary(*vt, vocabulary_path); @@ -420,6 +438,11 @@ void vocabulary_callback(const std_msgs::String::ConstPtr& msg) summary.load(vocabulary_path); summary.nbr_noise_segments += counter; summary.nbr_noise_sweeps++; + auto now = std::chrono::system_clock::now(); + auto in_time_t = std::chrono::system_clock::to_time_t(now); + char buffer [80]; + std::strftime(buffer, 80, "%Y-%m-%d %H:%M:%S", std::localtime(&in_time_t)); + summary.last_updated = string(buffer); summary.save(vocabulary_path); dynamic_object_retrieval::save_vocabulary(*vt, vocabulary_path); // End of new part! From a5dc46333bea27ee744bdec24c9bfdb56b632f2d Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 5 Sep 2016 17:49:59 +0200 Subject: [PATCH 087/255] Added launch file for the object search system --- .../launch/object_search.launch | 20 +++++++++++++++++++ .../src/quasimodo_retrieval_node.cpp | 3 +++ 2 files changed, 23 insertions(+) create mode 100644 quasimodo/quasimodo_object_search/launch/object_search.launch diff --git a/quasimodo/quasimodo_object_search/launch/object_search.launch b/quasimodo/quasimodo_object_search/launch/object_search.launch new file mode 100644 index 00000000..c6a46182 --- /dev/null +++ b/quasimodo/quasimodo_object_search/launch/object_search.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index 4d5cfbbe..1b47a0c2 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -151,6 +151,9 @@ class retrieval_node { if (summary.last_updated == temp_summary.last_updated) { return false; } + + cout << "Re-loading new vocabulary with timestamp: " << temp_summary.last_updated << endl; + summary = temp_summary; vt = grouped_vocabulary_tree(); dynamic_object_retrieval::load_vocabulary(vt, vocabulary_path); From 03cacfc381ff15639f881b6a1a5e6d36a92393c5 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 5 Sep 2016 18:15:23 +0200 Subject: [PATCH 088/255] I have brilliant commit messages, lolz --- .../metaroom_additional_view_processing.cpp | 4 +++- .../src/modelupdater/ModelUpdater.cpp | 18 ++++++++++-------- .../src/modelupdater/ModelUpdaterBasicFuse.cpp | 2 +- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 828a460b..0ddc5f80 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1049,6 +1049,7 @@ void sendCallback(const std_msgs::String::ConstPtr& msg){ int main(int argc, char** argv){ + bool once = false; const rlim_t kStackSize = 256 * 1024 * 1024; // min stack size = 256 MB struct rlimit rl; unsigned long result; @@ -1094,6 +1095,7 @@ int main(int argc, char** argv){ else if(std::string(argv[i]).compare("-sendModel") == 0){ inputstate = 8;} else if(std::string(argv[i]).compare("-sendSub") == 0) { inputstate = 9;} else if(std::string(argv[i]).compare("-sendTopic") == 0){ inputstate = 10;} + else if(std::string(argv[i]).compare("-once") == 0){ once = true;} else if(inputstate == 0){ if(sendsubs.size() == 0){ model_pub = n.advertise(modelouttopic, 1000); @@ -1133,7 +1135,7 @@ int main(int argc, char** argv){ } } - ros::spin(); + if(!once){ros::spin();} printf("done...\n"); return 0; } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index ac483891..cbba8999 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -340,7 +340,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M DistanceWeightFunction2 * func = new DistanceWeightFunction2(); func->f = THRESHOLD; - func->p = 0.01; + func->p = 0.02; Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} @@ -2515,9 +2515,13 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; - float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover); - float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover); - float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover); +// float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover); +// float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover); +// float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover); + + float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover) + minprob; + float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover) + minprob; + float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover) + 2.0*minprob; priors[3*(offset+ind)+0] = p_moving_tot; priors[3*(offset+ind)+1] = p_dynamic_tot; @@ -2527,9 +2531,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s // float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; // float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; -// float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover) + minprob; -// float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover) + minprob; -// float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover) + 2.0*minprob; + // priors[3*(offset+ind)+0] = p_moving_tot; // priors[3*(offset+ind)+1] = p_dynamic_tot; @@ -3907,7 +3909,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(RGBDFrame * src, ModelMask * DistanceWeightFunction2 * func = new DistanceWeightFunction2(); func->f = THRESHOLD; - func->p = 0.01; + func->p = 0.02; Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,residuals.size()); for(unsigned int i = 0; i < residuals.size(); i++){X(0,i) = residuals[i];} diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp index 83c6fd03..73207b58 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp @@ -78,7 +78,7 @@ printf("expectedCost: %f\n",expectedCost); unsigned int nr_models1 = models.size(); addModelsToVector(models,rps,model2,pose); - vector > ocs = computeOcclusionScore(models,rps,step,false); + vector > ocs = computeOcclusionScore(models,rps,step,false); std::vector > scores = getScores(ocs); std::vector partition = getPartition(scores,2,5,2); From f1e3da2fd913aea4eec28962de64f3a519168d65 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 5 Sep 2016 18:16:18 +0200 Subject: [PATCH 089/255] Trying to get this to run --- .../scripts/insert_object_server.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py index 8f7e1385..09410b08 100755 --- a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py +++ b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py @@ -23,14 +23,14 @@ from geometry_msgs.msg import Pose import time -def insert_model_cb(): +def insert_model_cb(req): msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') transforms = [] - req = mask_pointcloudsRequest() + mreq = mask_pointcloudsRequest() for cloud, mask, pose in zip(req.model.clouds, req.model.masks, req.model.local_poses): - req.clouds.append(cloud) - req.masks.append(mask) + mreq.clouds.append(cloud) + mreq.masks.append(mask) transform = Transform() transform.rotation.x = pose.quaternion.x @@ -46,7 +46,7 @@ def insert_model_cb(): rospy.wait_for_service('retrieval_segmentation_service') try: surfelize_server = rospy.ServiceProxy('retrieval_segmentation_service', mask_pointclouds) - resp = surfelize_server(req) + resp = surfelize_server(mreq) except rospy.ServiceException, e: print "Service call failed: %s"%e return False @@ -66,7 +66,7 @@ def insert_model_cb(): new_obj = fused_world_state_object() new_obj.surfel_cloud = resp.processed_cloud - new_obj.object_id = x.id + new_obj.object_id = resp.id now = datetime.now() new_obj.inserted_at = now.strftime("%Y-%m-%d %H:%M:%S") From c85abad000b1b662b9fd7449ad1c57a03f075880 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 5 Sep 2016 22:30:44 +0200 Subject: [PATCH 090/255] ... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 14 +++++++++----- quasimodo/quasimodo_brain/src/Util/Util.h | 8 ++++++-- .../src/modelupdater/ModelUpdater.cpp | 8 +++++--- .../src/modelupdater/ModelUpdaterBasicFuse.cpp | 2 ++ .../src/registration/MassRegistrationPPR2.cpp | 14 +++++++++++--- 5 files changed, 33 insertions(+), 13 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 4f46affc..98ecbdc2 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -56,7 +56,7 @@ reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg){ return model; } -void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp){ +void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp, bool addClouds){ int startsize = msg.local_poses.size(); msg.local_poses.resize(startsize+model->relativeposes.size()); msg.frames.resize(startsize+model->frames.size()); @@ -81,23 +81,27 @@ void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Af msg.frames[startsize+i].frame_id = model->frames[i]->id; msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); - msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg());//getMask() + msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg()); msg.frames[startsize+i].camera.K[0] = model->frames[i]->camera->fx; msg.frames[startsize+i].camera.K[4] = model->frames[i]->camera->fy; msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx; msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy; + sensor_msgs::PointCloud2 output; + pcl::toROSMsg(*(model->frames[i]->getPCLcloud()), output); + + msg.clouds.push_back(output); } for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ - addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp); + addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp,addClouds); } } -quasimodo_msgs::model getModelMSG(reglib::Model * model){ +quasimodo_msgs::model getModelMSG(reglib::Model * model, bool addClouds){ quasimodo_msgs::model msg; msg.model_id = model->id; - addToModelMSG(msg,model); + addToModelMSG(msg,model,Eigen::Affine3d::Identity(),addClouds); return msg; diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index f831ac30..53291c63 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -32,13 +32,17 @@ #include "metaroom_xml_parser/simple_xml_parser.h" +#include +#include +#include + namespace quasimodo_brain { double getTime(); reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg); -void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp = Eigen::Affine3d::Identity()); -quasimodo_msgs::model getModelMSG(reglib::Model * model); +void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp = Eigen::Affine3d::Identity(), bool addClouds = false); +quasimodo_msgs::model getModelMSG(reglib::Model * model, bool addClouds = false); std::vector getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index cbba8999..7b8594ba 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -239,6 +239,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); float * dst_normalsdata = (float *)(cf->normals.data); + unsigned char * dst_detdata = (unsigned char *)(cf->det_dilate.data); float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); @@ -298,6 +299,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); float dst_nx = dst_normalsdata[3*dst_ind+0]; if(dst_z > 0 && dst_nx != 2){ + if(dst_detdata[dst_ind] != 0){continue;} float dst_ny = dst_normalsdata[3*dst_ind+1]; float dst_nz = dst_normalsdata[3*dst_ind+2]; @@ -2309,7 +2311,7 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & if(dst_detdata[dst_ind] != 0){continue;} if(src_detdata[src_ind] != 0){continue;} - overlaps[src_ind] = std::max(overlaps[src_ind],p_overlap); + overlaps[src_ind] = std::min(0.9,std::max(overlaps[src_ind],p_overlap)); occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); } // if(fabs(d) < 0.02){//If close, according noises, and angle of the surfaces similar: FUSE @@ -2864,14 +2866,14 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s cloud_cluster2->points.push_back(p); } -// printf("score: %f avg: %f\n",score,score/totsum); + printf("score: %f avg: %f\n",score,score/totsum); if(score > 200){ for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ labels[dynamicdata[dynamic_indices[d].indices[ind]]] = 3; } } - if(false && cloud_cluster->points.size() > 500){ + if(false && cloud_cluster->points.size() > 500){ viewer->removeAllPointClouds(); viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); viewer->spin(); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp index 73207b58..f0ccdb90 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp @@ -110,6 +110,8 @@ printf("expectedCost: %f\n",expectedCost); printf("improvement: %f\n",improvement); if(improvement > best){ + computeOcclusionScore(models,rps,step,false); + best = improvement; best_id = ca; } diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index fc93fc01..82fdd4b3 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -527,10 +527,12 @@ void MassRegistrationPPR2::setData(std::vector frames_,std::vector & matchid, std::vector & matchdist, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ + printf("matchframes\n"); const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + //printf("nr_ap: %i\n",nr_ap); matchid.resize(nr_ap); matchdist.resize(nr_ap); @@ -544,6 +546,8 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ #pragma omp parallel for num_threads(8) #endif for(unsigned long k = 0; k < nr_ap; ++k) { + + //printf("k: %i\n",k); long prev = matchid[k]; double qp [3]; const double & src_x = ap[3*k+0]; @@ -554,6 +558,8 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ qp[1] = m10*src_x + m11*src_y + m12*src_z + m13; qp[2] = m20*src_x + m21*src_y + m22*src_z + m23; + //printf("qp: %f %f %f\n",qp[0],qp[1],qp[2]); + size_t ret_index; double out_dist_sqr; nanoflann::KNNResultSet resultSet(1); resultSet.init(&ret_index, &out_dist_sqr ); @@ -665,7 +671,7 @@ double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, l } void MassRegistrationPPR2::rematch(std::vector poses, std::vector prev_poses, bool first){ - //printf("rematch\n"); + printf("start rematch\n"); double new_good_rematches = 0; double new_total_rematches = 0; unsigned long nr_frames = poses.size(); @@ -713,14 +719,16 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect double ratiosum = 0; - if(use_depthedge){ + if(use_depthedge && depthedge_nr_arraypoints[i] > 10 && depthedge_nr_arraypoints[j] > 10){ // if(rand()%100 == 0){ // update_matchframes(depthedge_func,rp,depthedge_nr_arraypoints[i],depthedge_arraypoints[i],depthedge_matchids[i][j],depthedge_matchdists[i][j],depthedge_arraypoints[j],depthedge_nr_neighbours, depthedge_neighbours[j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); // exit(0); // } + //printf("use_depthedge\n"); ratiosum += matchframes(depthedge_func,rp, depthedge_nr_arraypoints[i], depthedge_arraypoints[i], depthedge_matchids[i][j], depthedge_matchdists[i][j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); } - if(use_surface){ + if(use_surface && nr_arraypoints[i] > 10 && nr_arraypoints[j] > 10){ + printf("use_surface\n"); ratiosum += matchframes(func,rp, nr_arraypoints[i], arraypoints[i], matchids[i][j], matchdists[i][j],trees3d[j], new_good_rematches,new_total_rematches); } From 79332f278ddd323f2ce3341b0481af008dadb778 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 6 Sep 2016 12:25:55 +0200 Subject: [PATCH 091/255] Fixed all bugs for Johan, but mongodb does not support inserting all the images since the size of the objects is too big --- .../scripts/insert_object_server.py | 39 +++++++++---------- 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py index 09410b08..74936178 100755 --- a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py +++ b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py @@ -23,20 +23,22 @@ from geometry_msgs.msg import Pose import time -def insert_model_cb(req): +def insert_model_cb(sreq): msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') + local_poses = sreq.model.local_poses + transforms = [] - mreq = mask_pointcloudsRequest() - for cloud, mask, pose in zip(req.model.clouds, req.model.masks, req.model.local_poses): - mreq.clouds.append(cloud) - mreq.masks.append(mask) + req = mask_pointcloudsRequest() + for cloud, mask, pose in zip(sreq.model.clouds, sreq.model.masks, sreq.model.local_poses): + req.clouds.append(cloud) + req.masks.append(mask) transform = Transform() - transform.rotation.x = pose.quaternion.x - transform.rotation.y = pose.quaternion.y - transform.rotation.z = pose.quaternion.z - transform.rotation.w = pose.quaternion.w + transform.rotation.x = pose.orientation.x + transform.rotation.y = pose.orientation.y + transform.rotation.z = pose.orientation.z + transform.rotation.w = pose.orientation.w transform.translation.x = pose.position.x transform.translation.y = pose.position.y transform.translation.z = pose.position.z @@ -46,7 +48,7 @@ def insert_model_cb(req): rospy.wait_for_service('retrieval_segmentation_service') try: surfelize_server = rospy.ServiceProxy('retrieval_segmentation_service', mask_pointclouds) - resp = surfelize_server(mreq) + resp = surfelize_server(req) except rospy.ServiceException, e: print "Service call failed: %s"%e return False @@ -66,19 +68,16 @@ def insert_model_cb(req): new_obj = fused_world_state_object() new_obj.surfel_cloud = resp.processed_cloud - new_obj.object_id = resp.id + new_obj.object_id = "null" now = datetime.now() new_obj.inserted_at = now.strftime("%Y-%m-%d %H:%M:%S") + new_obj.transforms = local_poses - for obs, pose in zip(wo._observations, wo._poses): - new_obj.transforms.append(Pose()) - new_obj.transforms[-1].position.x = pose.position.x - new_obj.transforms[-1].position.y = pose.position.y - new_obj.transforms[-1].position.z = pose.position.z - new_obj.transforms[-1].orientation.x = pose.quaternion.x - new_obj.transforms[-1].orientation.y = pose.quaternion.y - new_obj.transforms[-1].orientation.z = pose.quaternion.z - new_obj.transforms[-1].orientation.w = pose.quaternion.w + # TODO: mongodb does not support including all of this because of the size + # for rgbd_frame, mask in zip(sreq.model.frames, sreq.model.masks): + # new_obj.depths.append(rgbd_frame.depth) + # new_obj.images.append(rgbd_frame.rgb) + # new_obj.masks.append(mask) print "Waiting for retrieval_features_service..." rospy.wait_for_service('retrieval_features_service') From 9c0b1451c155cc9680a6d7c59f553807c73bda71 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 6 Sep 2016 13:49:46 +0200 Subject: [PATCH 092/255] fix segfault --- quasimodo/quasimodo_models/CMakeLists.txt | 2 +- .../include/registration/MassRegistration.h | 8 +- .../src/registration/MassRegistrationPPR2.cpp | 117 +----------------- .../src/registration/RegistrationRandom.cpp | 2 +- 4 files changed, 11 insertions(+), 118 deletions(-) diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index b55034c5..e17ffd61 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -3,7 +3,7 @@ project(quasimodo_models) #find_package(catkin REQUIRED)# quasimodo_msgs)# COMPONENTS quasimodo_msgs) #set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") -set(CMAKE_CXX_FLAGS "-O2 -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") +set(CMAKE_CXX_FLAGS "-O2 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") find_package(catkin REQUIRED COMPONENTS #metaroom_xml_parser diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistration.h b/quasimodo/quasimodo_models/include/registration/MassRegistration.h index 04074ceb..7b1ce234 100644 --- a/quasimodo/quasimodo_models/include/registration/MassRegistration.h +++ b/quasimodo/quasimodo_models/include/registration/MassRegistration.h @@ -13,10 +13,10 @@ #include #include #include -#include -#include -#include -#include +//#include +//#include +//#include +//#include #include //#include "ICP.h" diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 82fdd4b3..f68e544a 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -527,12 +527,10 @@ void MassRegistrationPPR2::setData(std::vector frames_,std::vector & matchid, std::vector & matchdist, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ - printf("matchframes\n"); const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); - //printf("nr_ap: %i\n",nr_ap); matchid.resize(nr_ap); matchdist.resize(nr_ap); @@ -540,26 +538,19 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ double good = 0; double bad = 0; - - #if defdomultithread #pragma omp parallel for num_threads(8) #endif for(unsigned long k = 0; k < nr_ap; ++k) { - - //printf("k: %i\n",k); long prev = matchid[k]; double qp [3]; const double & src_x = ap[3*k+0]; const double & src_y = ap[3*k+1]; const double & src_z = ap[3*k+2]; - qp[0] = m00*src_x + m01*src_y + m02*src_z + m03; qp[1] = m10*src_x + m11*src_y + m12*src_z + m13; qp[2] = m20*src_x + m21*src_y + m22*src_z + m23; - //printf("qp: %f %f %f\n",qp[0],qp[1],qp[2]); - size_t ret_index; double out_dist_sqr; nanoflann::KNNResultSet resultSet(1); resultSet.init(&ret_index, &out_dist_sqr ); @@ -569,109 +560,17 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ good++; }else{ bad++; - } - + } long current = ret_index; new_good_rematches += prev != current; new_total_rematches++; matchid[k] = current; matchdist[k] = out_dist_sqr; } - return good/(good+bad+0.001); } -double update_matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ap, double * ap, std::vector & matchid, std::vector & matchdist, double * apj, long nr_dn, long * dn, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ - const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); - const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); - const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); - - matchid.resize(nr_ap); - matchdist.resize(nr_ap); - - double threshold = pow(10*f->getNoise(),2); - double good = 0; - double bad = 0; - - - -#if defdomultithread - #pragma omp parallel for num_threads(8) -#endif - for(unsigned long k = 0; k < nr_ap; ++k) { - const double & src_x = ap[3*k+0]; - const double & src_y = ap[3*k+1]; - const double & src_z = ap[3*k+2]; - - const double & sx = m00*src_x + m01*src_y + m02*src_z + m03; - const double & sy = m10*src_x + m11*src_y + m12*src_z + m13; - const double & sz = m20*src_x + m21*src_y + m22*src_z + m23; - - long prev = matchid[k]; - const double & dx1 =apj[3*prev+0]-sx; - const double & dy1 =apj[3*prev+1]-sy; - const double & dz1 =apj[3*prev+2]-sz; - - double before = dx1*dx1+dy1*dy1+dz1*dz1;; - double mindist = before; - - printf("point %4.4i before: %5.5f -> ",k,before); - while(true){ - long next = prev; - for(unsigned long m = 0; m < nr_dn; ++m) { - long id = dn[nr_dn*prev+m]; - const double & dx =apj[3*id+0]-sx; - const double & dy =apj[3*id+1]-sy; - const double & dz =apj[3*id+2]-sz; - const double sqr_dist = dx*dx+dy*dy+dz*dz; - if(sqr_dist < mindist){ - mindist = sqr_dist; - next = id; - } - } - printf("%5.5f ",mindist); - if(next == prev){break;} - prev = next; - } - - double qp [3]; - qp[0] = sx; - qp[1] = sy; - qp[2] = sz; - size_t ret_index; double out_dist_sqr; - nanoflann::KNNResultSet resultSet(1); - resultSet.init(&ret_index, &out_dist_sqr ); - t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); - - const double & dx2 =apj[3*ret_index+0]-sx; - const double & dy2 =apj[3*ret_index+1]-sy; - const double & dz2 =apj[3*ret_index+2]-sz; - - double gt = dx2*dx2+dy2*dy2+dz2*dz2; - - //printf("point %4.4i before: %5.5f after: %5.5f gt: %5.5f\n",k,before,mindist,out_dist_sqr); - printf(" gt: %5.5f gt: %5.5f\n",out_dist_sqr,gt); - - - -// if(out_dist_sqr < threshold){ -// good++; -// }else{ -// bad++; -// } - -// long current = ret_index; -// new_good_rematches += prev != current; -// new_total_rematches++; -// matchid[k] = current; -// matchdist[k] = out_dist_sqr; - } - - return 1; -} - void MassRegistrationPPR2::rematch(std::vector poses, std::vector prev_poses, bool first){ - printf("start rematch\n"); double new_good_rematches = 0; double new_total_rematches = 0; unsigned long nr_frames = poses.size(); @@ -720,23 +619,15 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect double ratiosum = 0; if(use_depthedge && depthedge_nr_arraypoints[i] > 10 && depthedge_nr_arraypoints[j] > 10){ -// if(rand()%100 == 0){ -// update_matchframes(depthedge_func,rp,depthedge_nr_arraypoints[i],depthedge_arraypoints[i],depthedge_matchids[i][j],depthedge_matchdists[i][j],depthedge_arraypoints[j],depthedge_nr_neighbours, depthedge_neighbours[j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); -// exit(0); -// } - //printf("use_depthedge\n"); ratiosum += matchframes(depthedge_func,rp, depthedge_nr_arraypoints[i], depthedge_arraypoints[i], depthedge_matchids[i][j], depthedge_matchdists[i][j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); - } + } if(use_surface && nr_arraypoints[i] > 10 && nr_arraypoints[j] > 10){ - printf("use_surface\n"); ratiosum += matchframes(func,rp, nr_arraypoints[i], arraypoints[i], matchids[i][j], matchdists[i][j],trees3d[j], new_good_rematches,new_total_rematches); } matchscores[i][j] = ratiosum; - //printf("%i %i -> %5.5f\n",i,j,ratiosum); overlapping += ratiosum > 0; - work_done++; - + work_done++; } } @@ -1051,6 +942,8 @@ Eigen::MatrixXd MassRegistrationPPR2::depthedge_getAllResiduals(std::vector & matchidi = depthedge_matchids[i][j]; unsigned long matchesi = matchidi.size(); + if(matchesi != nr_api){continue;} + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp index c0c49426..6da427f4 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp @@ -168,7 +168,7 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ #pragma omp parallel for num_threads(8) for(unsigned int r = 0; r < nr_r; r++){ - printf("registering: %i / %i\n",r+1,nr_r); + //printf("registering: %i / %i\n",r+1,nr_r); double start = getTime(); double meantime = 999999999999; From 9baa458e2d422bcc21094bf076c552494201a4f3 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 6 Sep 2016 15:06:49 +0200 Subject: [PATCH 093/255] ... --- .../src/ModelDatabase/ModelDatabaseRetrieval.cpp | 4 ++-- quasimodo/quasimodo_brain/src/Util/Util.cpp | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp index 81093d6c..4a9f8124 100644 --- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp +++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp @@ -14,7 +14,7 @@ ModelDatabaseRetrieval::~ModelDatabaseRetrieval(){} bool ModelDatabaseRetrieval::add(reglib::Model * model){ quasimodo_msgs::insert_model im; - im.request.model = quasimodo_brain::getModelMSG(model); + im.request.model = quasimodo_brain::getModelMSG(model,true); im.request.action = im.request.INSERT; if (insert_client.call(im)){ object_ids.push_back(im.response.object_id); @@ -58,7 +58,7 @@ std::vector ModelDatabaseRetrieval::search(reglib::Model * mode std::vector ret; quasimodo_msgs::model_to_retrieval_query m2r; - m2r.request.model = quasimodo_brain::getModelMSG(model); + m2r.request.model = quasimodo_brain::getModelMSG(model,true); if (conversion_client.call(m2r)){ quasimodo_msgs::query_cloud qc; qc.request.query = m2r.response.query; diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 98ecbdc2..7f0520b0 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -88,8 +88,10 @@ void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Af msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx; msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy; + if(addClouds){} sensor_msgs::PointCloud2 output; pcl::toROSMsg(*(model->frames[i]->getPCLcloud()), output); + printf("outputcloud: %i %i\n",output.point_step,output.row_step); msg.clouds.push_back(output); } From 6bb01678016172c005063409bfb49c3e80317e67 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Wed, 7 Sep 2016 10:02:11 +0200 Subject: [PATCH 094/255] Checking if message not correct before returning --- .../dynamic_object_retrieval/dynamic_retrieval.h | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index ea9ad3e1..3dbb1540 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -149,6 +149,12 @@ std::vector get_retrieved_paths(const std::vector message; mongo::BSONObj bson_message; std::tie(message, bson_message) = message_store->queryID(strs[2]); + + if (message->transforms.empty()) { + retrieved_paths.push_back(boost::filesystem::path()); + continue; + } + SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT); pcl::fromROSMsg(message->surfel_cloud, *surfel_cloud); pcl::io::savePCDFileBinary(temp_path.string(), *surfel_cloud); @@ -373,6 +379,12 @@ get_retrieved_path_scores(const std::vector::r boost::shared_ptr message; mongo::BSONObj bson_message; std::tie(message, bson_message) = message_store->queryID(strs[2]); + + if (message->transforms.empty()) { + path_scores.back().first.push_back(boost::filesystem::path()); + continue; + } + SurfelCloudT::Ptr surfel_cloud(new SurfelCloudT); pcl::fromROSMsg(message->surfel_cloud, *surfel_cloud); pcl::io::savePCDFileBinary(temp_path.string(), *surfel_cloud); From 49eccd9a6c73c66a5eb3c32bf908d30908d25b6c Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Wed, 7 Sep 2016 18:07:41 +0200 Subject: [PATCH 095/255] Added the vocabulary id to retrieval results --- quasimodo/quasimodo_msgs/msg/retrieval_result.msg | 1 + .../quasimodo_retrieval/src/quasimodo_retrieval_node.cpp | 9 +++++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_msgs/msg/retrieval_result.msg b/quasimodo/quasimodo_msgs/msg/retrieval_result.msg index 37f3faba..a013a2f6 100644 --- a/quasimodo/quasimodo_msgs/msg/retrieval_result.msg +++ b/quasimodo/quasimodo_msgs/msg/retrieval_result.msg @@ -7,3 +7,4 @@ string_array[] retrieved_image_paths float64[] retrieved_distance_scores int_array[] segment_indices geometry_msgs/Pose[] global_poses +uint64[] vocabulary_ids diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index 1b47a0c2..28fba39d 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -576,6 +576,7 @@ class retrieval_node { vector scores; vector indices; + vector vocabulary_ids; for (auto s : results.first) { boost::filesystem::path segment_path = base_path(s.first); string name = segment_path.stem().string(); @@ -589,6 +590,7 @@ class retrieval_node { indices.push_back(index); } scores.push_back(s.second.score); + vocabulary_ids.push_back(s.second.index); } vector, Eigen::aligned_allocator > initial_poses; @@ -620,7 +622,7 @@ class retrieval_node { tf::transformTFToMsg(room_transform, query_room_transform); - result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices); + result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices, vocabulary_ids); for (int i = 0; i < retrieved_clouds.size(); ++i) { sensor_msgs::PointCloud2 cloud_msg = result.retrieved_clouds[i]; cloud_msg.header.stamp = ros::Time::now(); @@ -708,7 +710,8 @@ class retrieval_node { const vector >& masks, const vector >& paths, const vector& scores, - const vector& indices) + const vector& indices, + const vector& vocabulary_ids) { quasimodo_msgs::retrieval_result res; @@ -734,6 +737,7 @@ class retrieval_node { res.retrieved_image_paths.resize(number_retrieved); res.retrieved_distance_scores.resize(number_retrieved); res.segment_indices.resize(number_retrieved); + res.vocabulary_ids.resize(number_retrieved); for (int i = 0; i < number_retrieved; ++i) { pcl::toROSMsg(*clouds[i], res.retrieved_clouds[i]); @@ -754,6 +758,7 @@ class retrieval_node { } res.retrieved_distance_scores[i] = scores[i]; res.segment_indices[i].ints.push_back(indices[i]); + res.vocabulary_ids[i] = vocabulary_ids[i]; } return res; From 99d7177a0e76373edd369bf89b8ee64bf6380438 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Wed, 7 Sep 2016 18:08:11 +0200 Subject: [PATCH 096/255] Also outputting the individual images from the visualization server --- .../benchmark_visualization.h | 7 ++-- .../benchmark/src/benchmark_visualization.cpp | 26 +++++++++++--- .../quasimodo_msgs/srv/visualize_query.srv | 1 + .../src/quasimodo_visualization_server.cpp | 34 +++++++++++++------ 4 files changed, 51 insertions(+), 17 deletions(-) diff --git a/dynamic_object_retrieval/benchmark/include/object_3d_benchmark/benchmark_visualization.h b/dynamic_object_retrieval/benchmark/include/object_3d_benchmark/benchmark_visualization.h index c16e9136..dce92ab6 100644 --- a/dynamic_object_retrieval/benchmark/include/object_3d_benchmark/benchmark_visualization.h +++ b/dynamic_object_retrieval/benchmark/include/object_3d_benchmark/benchmark_visualization.h @@ -16,8 +16,8 @@ using CloudT = pcl::PointCloud; namespace benchmark_retrieval { void put_text(cv::Mat& image, const std::string& text); -cv::Mat make_image(std::vector& results, const Eigen::Matrix4f& room_transform, - std::vector& sweep_paths, const std::vector& optional_text); +std::pair > make_image(std::vector& results, const Eigen::Matrix4f& room_transform, + std::vector& sweep_paths, const std::vector& optional_text); cv::Mat add_query_image(cv::Mat& results, cv::Mat& query_image, const std::string& query_label); // this will alter the look of the point clouds, but for the better I think @@ -25,6 +25,9 @@ cv::Mat add_query_image(cv::Mat& results, cv::Mat& query_image, const std::strin cv::Mat make_visualization_image(cv::Mat& query_image, const std::string& query_label, std::vector& clouds, std::vector& sweep_paths, const std::vector& optional_text, const Eigen::Matrix4f& T); +cv::Mat make_visualization_image(cv::Mat& query_image, const std::string& query_label, std::vector& clouds, + std::vector& sweep_paths, const std::vector& optional_text, + const Eigen::Matrix4f& T, std::vector& individual_images); cv::Mat make_visualization_image(CloudT::Ptr& query_cloud, cv::Mat& query_mask, const boost::filesystem::path& query_path, const Eigen::Matrix3f& K, const Eigen::Matrix4f& query_transform, const std::string& query_label, std::vector& clouds, diff --git a/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp b/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp index 4069926d..d447f78a 100644 --- a/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp +++ b/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp @@ -28,11 +28,23 @@ cv::Mat make_visualization_image(cv::Mat& query_image, const string& query_label p.getVector4fMap() = T*p.getVector4fMap(); } }*/ - cv::Mat result_image = make_image(clouds, T, sweep_paths, optional_text); + cv::Mat result_image; + vector individual_images; + tie(result_image, individual_images) = make_image(clouds, T, sweep_paths, optional_text); return add_query_image(result_image, query_image, query_label); //return result_image; } +cv::Mat make_visualization_image(cv::Mat& query_image, const string& query_label, vector& clouds, + vector& sweep_paths, const vector& optional_text, + const Eigen::Matrix4f& T, vector& individual_images) +{ + cv::Mat result_image; + tie(result_image, individual_images) = make_image(clouds, T, sweep_paths, optional_text); + individual_images.insert(individual_images.begin(), result_image.clone()); + return add_query_image(result_image, query_image, query_label); +} + cv::Mat make_visualization_image(CloudT::Ptr& query_cloud, cv::Mat& query_mask, const boost::filesystem::path& query_path, const Eigen::Matrix3f& K, const Eigen::Matrix4f& query_transform, const string& query_label, vector& clouds, @@ -70,7 +82,9 @@ cv::Mat make_visualization_image(CloudT::Ptr& query_cloud, cv::Mat& query_mask, cv::Mat inverted_mask; cv::bitwise_not(query_mask, inverted_mask); image.setTo(cv::Scalar(255, 255, 255), inverted_mask); - cv::Mat result_image = make_image(clouds, T, sweep_paths, optional_text); + cv::Mat result_image; + vector individual_images; + tie(result_image, individual_images) = make_image(clouds, T, sweep_paths, optional_text); return add_query_image(result_image, image, query_label); //return result_image; } @@ -244,8 +258,8 @@ cv::Mat render_image(CloudT::Ptr& cloud, const Eigen::Matrix4f& T, const Eigen:: } // the question is if we actually need the paths for this? -cv::Mat make_image(std::vector& results, const Eigen::Matrix4f& room_transform, - vector& sweep_paths, const std::vector& optional_text) +pair > make_image(std::vector& results, const Eigen::Matrix4f& room_transform, + vector& sweep_paths, const std::vector& optional_text) { pair sizes = make_pair(1, std::min(10, int(results.size())));//get_similar_sizes(results.size()); @@ -253,6 +267,7 @@ cv::Mat make_image(std::vector& results, const Eigen::Matrix4f& roo int height = 200; cv::Mat visualization = cv::Mat::zeros(height*sizes.first, width*sizes.second, CV_8UC3); + cv::Mat individual_images; int counter = 0; for (CloudT::Ptr& cloud : results) { @@ -327,6 +342,7 @@ cv::Mat make_image(std::vector& results, const Eigen::Matrix4f& roo int offset_height = counter / sizes.second; int offset_width = counter % sizes.second; sub_image.copyTo(visualization(cv::Rect(offset_width*width, offset_height*height, width, height))); + individual_images.push_back(sub_image); //cv::imshow("test", visualization); //cv::waitKey(); @@ -338,7 +354,7 @@ cv::Mat make_image(std::vector& results, const Eigen::Matrix4f& roo visualization = cv::Mat::zeros(height*2, width*5, CV_8UC3); } - return visualization; + return make_pair(visualization, individual_images); } } // namespace benchmark_retrieval diff --git a/quasimodo/quasimodo_msgs/srv/visualize_query.srv b/quasimodo/quasimodo_msgs/srv/visualize_query.srv index 53cde70e..a10f907a 100644 --- a/quasimodo/quasimodo_msgs/srv/visualize_query.srv +++ b/quasimodo/quasimodo_msgs/srv/visualize_query.srv @@ -2,3 +2,4 @@ retrieval_query query retrieval_result result --- sensor_msgs/Image image +quasimodo_msgs/image_array images diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_visualization_server.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_visualization_server.cpp index 84596088..fb9c2c75 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_visualization_server.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_visualization_server.cpp @@ -54,8 +54,8 @@ class visualization_server { //sensor_msgs/Image image //sensor_msgs/CameraInfo camera //geometry_msgs/Transform room_transform - sensor_msgs::Image vis_img_from_msgs(const quasimodo_msgs::retrieval_query& query, - const quasimodo_msgs::retrieval_result& result) + quasimodo_msgs::visualize_query::Response vis_img_from_msgs(const quasimodo_msgs::retrieval_query& query, + const quasimodo_msgs::retrieval_result& result) { cv_bridge::CvImagePtr cv_ptr; try { @@ -125,19 +125,33 @@ class visualization_server { label_pub.publish(label_msg); - cv::Mat visualization = benchmark_retrieval::make_visualization_image(cv_ptr->image, query_label, retrieved_clouds, sweep_paths, dummy_labels, T); + vector individual_images; + cv::Mat visualization = benchmark_retrieval::make_visualization_image(cv_ptr->image, query_label, retrieved_clouds, + sweep_paths, dummy_labels, T, individual_images); - cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage); - cv_pub_ptr->image = visualization; - cv_pub_ptr->encoding = "bgr8"; - return *cv_pub_ptr->toImageMsg(); + + quasimodo_msgs::visualize_query::Response resp; + { + cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage); + cv_pub_ptr->image = visualization; + cv_pub_ptr->encoding = "bgr8"; + resp.image = *cv_pub_ptr->toImageMsg(); + } + for (cv::Mat& im : individual_images) { + cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage); + cv_pub_ptr->image = im; + cv_pub_ptr->encoding = "bgr8"; + resp.images.images.push_back(*cv_pub_ptr->toImageMsg()); + } + + return resp; } bool service_callback(quasimodo_msgs::visualize_query::Request& req, quasimodo_msgs::visualize_query::Response& res) { - res.image = vis_img_from_msgs(req.query, req.result); + res = vis_img_from_msgs(req.query, req.result); image_pub.publish(res.image); @@ -146,9 +160,9 @@ class visualization_server { void callback(const quasimodo_msgs::retrieval_query_result& res) { - sensor_msgs::Image image = vis_img_from_msgs(res.query, res.result); + quasimodo_msgs::visualize_query::Response resp = vis_img_from_msgs(res.query, res.result); - image_pub.publish(image); + image_pub.publish(resp.image); } }; From 84e07433c8077a53d5d05d16fe77e54ef46f8896 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 7 Sep 2016 22:34:38 +0200 Subject: [PATCH 097/255] ... --- .../ModelDatabase/ModelDatabaseRetrieval.cpp | 38 ++++- quasimodo/quasimodo_brain/src/Util/Util.cpp | 7 +- .../metaroom_additional_view_processing.cpp | 42 +++-- quasimodo/quasimodo_brain/src/modelserver.cpp | 99 ++++++++---- .../quasimodo_models/include/core/Util.h | 75 +++------ .../include/modelupdater/ModelUpdater.h | 19 ++- .../quasimodo_models/src/core/Camera.cpp | 2 - .../quasimodo_models/src/core/RGBDFrame.cpp | 2 +- quasimodo/quasimodo_models/src/core/Util.cpp | 4 + .../quasimodo_models/src/model/Model.cpp | 2 +- .../src/modelupdater/ModelUpdater.cpp | 147 ++++++++++++++---- .../modelupdater/ModelUpdaterBasicFuse.cpp | 64 ++++---- .../src/registration/MassRegistrationPPR2.cpp | 19 +-- 13 files changed, 329 insertions(+), 191 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp index 4a9f8124..5c2a3b59 100644 --- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp +++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp @@ -1,4 +1,5 @@ #include "ModelDatabaseRetrieval.h" +//#include "mongo/client/dbclient.h" using namespace std; @@ -8,6 +9,10 @@ ModelDatabaseRetrieval::ModelDatabaseRetrieval(ros::NodeHandle & n, std::string retrieval_client = n.serviceClient (retrieval_name); conversion_client = n.serviceClient (conversion_name); insert_client = n.serviceClient (insert_name); + + //mongo::DBClientConnection c; + //c.connect("localhost"); + //c.dropCollection("databaseName.collectionName"); } ModelDatabaseRetrieval::~ModelDatabaseRetrieval(){} @@ -21,7 +26,7 @@ bool ModelDatabaseRetrieval::add(reglib::Model * model){ models.push_back(model); vocabulary_ids[im.response.vocabulary_id] = model; v_ids.push_back(im.response.vocabulary_id); - printf("number of models: %i\n",models.size()); + printf("ADD: number of models total added to database: %6.6i last added vocabulary_id: %6.6i last added object_ids: %s\n",models.size(),v_ids.back(),object_ids.back().c_str()); return true; }else{ ROS_ERROR("insert_client service INSERT FAIL!"); @@ -40,10 +45,13 @@ bool ModelDatabaseRetrieval::remove(reglib::Model * model){ vocabulary_ids.erase(v_ids[i]); models[i] = models.back(); models.pop_back(); + std::string oid = object_ids[i]; object_ids[i] = object_ids.back(); object_ids.pop_back(); + int vid = v_ids[i]; v_ids[i] = v_ids.back(); v_ids.pop_back(); + printf("REMOVE: number of models in database: %6.6i removed vocabulary_id: %6.6i last added object_ids: %s\n",models.size(),vid,oid.c_str()); return true; }else{ ROS_ERROR("insert_client service REMOVE FAIL!"); @@ -55,6 +63,13 @@ bool ModelDatabaseRetrieval::remove(reglib::Model * model){ } std::vector ModelDatabaseRetrieval::search(reglib::Model * model, int number_of_matches){ + + printf("---------------ids in database: %i----------------\n",v_ids.size()); + for(unsigned int i = 0; i < v_ids.size(); i++){ + printf("v_ids[%i] = %i\n",i,v_ids[i]); + } + printf("----------------------------------------------\n"); + std::vector ret; quasimodo_msgs::model_to_retrieval_query m2r; @@ -63,14 +78,25 @@ std::vector ModelDatabaseRetrieval::search(reglib::Model * mode quasimodo_msgs::query_cloud qc; qc.request.query = m2r.response.query; qc.request.query.query_kind = qc.request.query.MONGODB_QUERY; - qc.request.query.number_query = number_of_matches; + qc.request.query.number_query = number_of_matches+10; if (retrieval_client.call(qc)){ quasimodo_msgs::retrieval_result result = qc.response.result; - for(unsigned int i = 0; i < result.segment_indices.size(); i++){ - reglib::Model * search_model = vocabulary_ids[result.segment_indices[i].ints.front()]; - if(search_model != model){ret.push_back(search_model);} - if(ret.size() == number_of_matches){return ret;} + + printf("---------------ids in searchresult: %i----------------\n",result.vocabulary_ids.size()); + for(unsigned int i = 0; i < result.vocabulary_ids.size(); i++){ + int ind = result.vocabulary_ids[i]; + printf("found object with ind: %i\n",ind); + } + printf("----------------------------------------------\n"); + + for(unsigned int i = 0; i < result.vocabulary_ids.size(); i++){ + int ind = result.vocabulary_ids[i]; + printf("found object with ind: %i\n",ind); + if(vocabulary_ids.count(ind) == 0){ printf("does not exist in db, continue\n"); continue; } + reglib::Model * search_model = vocabulary_ids[ind]; + if(search_model != model){printf("adding to search results\n"); ret.push_back(search_model);} + if(ret.size() == number_of_matches){printf("found enough search results\n"); return ret;} } }else{ ROS_ERROR("retrieval_client service FAIL!"); diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 7f0520b0..5ac6739a 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -88,11 +88,8 @@ void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Af msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx; msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy; - if(addClouds){} sensor_msgs::PointCloud2 output; - pcl::toROSMsg(*(model->frames[i]->getPCLcloud()), output); - printf("outputcloud: %i %i\n",output.point_step,output.row_step); - + if(addClouds){pcl::toROSMsg(*(model->frames[i]->getPCLcloud()), output);} msg.clouds.push_back(output); } for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ @@ -104,8 +101,6 @@ quasimodo_msgs::model getModelMSG(reglib::Model * model, bool addClouds){ quasimodo_msgs::model msg; msg.model_id = model->id; addToModelMSG(msg,model,Eigen::Affine3d::Identity(),addClouds); - - return msg; } diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 0ddc5f80..d33695f8 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -179,11 +179,14 @@ void writeXml(std::string xmlFile, std::vector & frames, st xmlWriter->writeStartElement("View"); sprintf(buf,"%s/view_RGB%10.10i.png",sweep_folder.c_str(),i); cv::imwrite(buf, frame->rgb ); + + sprintf(buf,"view_RGB%10.10i.png",i); xmlWriter->writeAttribute("RGB", QString(buf)); sprintf(buf,"%s/view_DEPTH%10.10i.png",sweep_folder.c_str(),i); cv::imwrite(buf, frame->depth ); + sprintf(buf,"view_DEPTH%10.10i.png",i); xmlWriter->writeAttribute("DEPTH", QString(buf)); long nsec = 1e9*((frame->capturetime)-std::floor(frame->capturetime)); @@ -289,7 +292,7 @@ void readViewXML(std::string xmlFile, std::vector & frames, QString xmlFileQS(xmlFile.c_str()); int index = xmlFileQS.lastIndexOf('/'); - QString roomFolder = xmlFileQS.left(index); + std::string roomFolder = xmlFileQS.left(index).toStdString(); file.open(QIODevice::ReadOnly); @@ -323,17 +326,25 @@ void readViewXML(std::string xmlFile, std::vector & frames, QXmlStreamAttributes attributes = xmlReader->attributes(); if (attributes.hasAttribute("RGB")) { - QString rgbpath = attributes.value("RGB").toString(); - //printf("rgb filename: %s\n",rgbpath.toStdString().c_str()); - rgb = cv::imread(rgbpath.toStdString().c_str(), CV_LOAD_IMAGE_UNCHANGED); + std::string imgpath = attributes.value("RGB").toString().toStdString(); + printf("rgb filename: %s\n",(roomFolder+"/"+imgpath).c_str()); + rgb = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); + + //QString rgbpath = attributes.value("RGB").toString(); + //rgb = cv::imread(roomFolder+"/"+(rgbpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); + //rgb = cv::imread((rgbpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); }else{break;} if (attributes.hasAttribute("DEPTH")) { - QString depthpath = attributes.value("DEPTH").toString(); + std::string imgpath = attributes.value("DEPTH").toString().toStdString(); + printf("depth filename: %s\n",(roomFolder+"/"+imgpath).c_str()); + depth = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); + //QString depthpath = attributes.value("DEPTH").toString(); //printf("depth filename: %s\n",depthpath.toStdString().c_str()); - depth = cv::imread(depthpath.toStdString().c_str(), CV_LOAD_IMAGE_UNCHANGED); + //depth = cv::imread((roomFolder+"/"+depthpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); + //depth = cv::imread(roomFolder+"/"+(depthpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); }else{break;} @@ -539,7 +550,7 @@ void savePoses(std::string xmlFile, std::vector poses, int maxp xmlWriter->writeStartDocument(); xmlWriter->writeStartElement("Poses"); // xmlWriter->writeAttribute("number_of_poses", QString::number(poses.size())); - for(unsigned int i = 0; i < poses.size() && (i == -1 || i < maxposes); i++){ + for(unsigned int i = 0; i < poses.size() && (maxposes == -1 || int(i) < maxposes); i++){ printf("saving %i\n",i); xmlWriter->writeStartElement("Pose"); writePose(xmlWriter,poses[i]); @@ -627,7 +638,7 @@ void processMetaroom(std::string path){ int prevind = -1; std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); - for (int i = 0; i < sweep_xmls.size(); i++){ + for (unsigned int i = 0; i < sweep_xmls.size(); i++){ SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); std::string other_waypointid = other_roomData.roomWaypointId; @@ -753,7 +764,7 @@ printf("%s::%i\n",__FILE__,__LINE__); // viewer->spin(); // } printf("%s::%i\n",__FILE__,__LINE__); - printf("dynamiccloud: %i\n",dynamiccloud->points.size()); + printf("dynamiccloud: %i\n",int(dynamiccloud->points.size())); if(dynamiccloud->points.size() > 0){ printf("%s::%i\n",__FILE__,__LINE__); // Creating the KdTree object for the search method of the extraction @@ -806,6 +817,8 @@ printf("%s::%i\n",__FILE__,__LINE__); char buf [1024]; sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),d,j); cv::imwrite(buf, mask ); + + sprintf(buf,"dynamicmask_%i_%i.png",d,j); xmlWriter->writeStartElement("Mask"); xmlWriter->writeAttribute("filename", QString(buf)); xmlWriter->writeAttribute("image_number", QString::number(j)); @@ -819,7 +832,7 @@ printf("%s::%i\n",__FILE__,__LINE__); } - printf("movingcloud: %i\n",movingcloud->points.size()); + printf("movingcloud: %i\n",int(movingcloud->points.size())); if(movingcloud->points.size() > 0){ // Creating the KdTree object for the search method of the extraction pcl::search::KdTree::Ptr movingtree (new pcl::search::KdTree); @@ -996,8 +1009,9 @@ std::vector loadModels(std::string path){ QXmlStreamAttributes attributes = xmlReader->attributes(); if (attributes.hasAttribute("filename")){ QString maskpath = attributes.value("filename").toString(); - printf("mask filename: %s\n",maskpath.toStdString().c_str()); - mask = cv::imread(maskpath.toStdString().c_str(), CV_LOAD_IMAGE_UNCHANGED); + //printf("mask filename: %s\n",(sweep_folder+maskpath.toStdString()).c_str()); + mask = cv::imread(sweep_folder+"/"+(maskpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); + //mask = cv::imread((maskpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); }else{break;} @@ -1006,8 +1020,6 @@ std::vector loadModels(std::string path){ QString depthpath = attributes.value("image_number").toString(); number = atoi(depthpath.toStdString().c_str()); printf("number: %i\n",number); - - }else{break;} mod->frames.push_back(frames[number]->clone()); @@ -1061,7 +1073,7 @@ int main(int argc, char** argv){ rl.rlim_cur = kStackSize; result = setrlimit(RLIMIT_STACK, &rl); if (result != 0){ - fprintf(stderr, "setrlimit returned result = %d\n", result); + fprintf(stderr, "setrlimit returned result = %d\n", int(result)); } //printf("result: %ld mb and %ld mb\n",rl.rlim_cur/(1024*1024),rl.rlim_max/(1024*1024)); } diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index b331eb10..b1d0a09a 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -60,6 +60,7 @@ int show_init_lvl = 0;//init show int show_refine_lvl = 0;//refine show int show_reg_lvl = 0;//registration show bool show_scoring = false;//fuse scoring show +bool show_search = false; std::map cameras; @@ -161,11 +162,11 @@ void dumpDatabase(std::string path = "."){ char command [1024]; sprintf(command,"rm -r -f %s/database_tmp",path.c_str()); - printf("%s\n",command); + //printf("%s\n",command); int r = system(command); sprintf(command,"mkdir %s/database_tmp",path.c_str()); - printf("%s\n",command); + //printf("%s\n",command); r = system(command); for(unsigned int m = 0; m < modeldatabase->models.size(); m++){ @@ -210,22 +211,12 @@ void retrievalCallback(const quasimodo_msgs::retrieval_query_result & qr){ last_search_result_time = getTime(); } - -int savecounter = 0; -void show_sorted(){ - printf("show_sorted\n"); - if(!show_db){return;} - if(!visualization){return;} - std::vector results; - for(unsigned int i = 0; i < modeldatabase->models.size(); i++){results.push_back(modeldatabase->models[i]);} - std::sort (results.begin(), results.end(), myfunction); +void showModels(std::vector mods){ viewer->removeAllPointClouds(); float maxx = 0; - - printf("pre->loop\n"); - for(unsigned int i = 0; i < results.size(); i++){ - pcl::PointCloud::Ptr cloud = results[i]->getPCLcloud(1, false); + for(unsigned int i = 0; i < mods.size(); i++){ + pcl::PointCloud::Ptr cloud = mods[i]->getPCLcloud(1, false); float meanx = 0; float meany = 0; float meanz = 0; @@ -254,6 +245,52 @@ void show_sorted(){ viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); } viewer->spin(); +} + +int savecounter = 0; +void show_sorted(){ + //printf("show_sorted\n"); + if(!show_db){return;} + if(!visualization){return;} + std::vector results; + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){results.push_back(modeldatabase->models[i]);} + std::sort (results.begin(), results.end(), myfunction); + showModels(results); + +// viewer->removeAllPointClouds(); +// float maxx = 0; + +// printf("pre->loop\n"); +// for(unsigned int i = 0; i < results.size(); i++){ +// pcl::PointCloud::Ptr cloud = results[i]->getPCLcloud(1, false); +// float meanx = 0; +// float meany = 0; +// float meanz = 0; +// for(unsigned int j = 0; j < cloud->points.size(); j++){ +// meanx += cloud->points[j].x; +// meany += cloud->points[j].y; +// meanz += cloud->points[j].z; +// } +// meanx /= float(cloud->points.size()); +// meany /= float(cloud->points.size()); +// meanz /= float(cloud->points.size()); + +// for(unsigned int j = 0; j < cloud->points.size(); j++){ +// cloud->points[j].x -= meanx; +// cloud->points[j].y -= meany; +// cloud->points[j].z -= meanz; +// } + +// float minx = 100000000000; + +// for(unsigned int j = 0; j < cloud->points.size(); j++){minx = std::min(cloud->points[j].x , minx);} +// for(unsigned int j = 0; j < cloud->points.size(); j++){cloud->points[j].x += maxx-minx + 0.15;} +// for(unsigned int j = 0; j < cloud->points.size(); j++){maxx = std::max(cloud->points[j].x,maxx);} +// char buf [1024]; +// sprintf(buf,"cloud%i",i); +// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); +// } +// viewer->spin(); //char buf [1024]; //sprintf(buf,"globalimg%i.png",savecounter++); //viewer->saveScreenshot(std::string(buf)); @@ -460,7 +497,7 @@ void call_from_thread(int i) { } void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, bool deleteIfFail = false){ - printf("addToDB %i %i\n",int(add),int(deleteIfFail)); + //printf("addToDB %i %i\n",int(add),int(deleteIfFail)); if(add){ if(model->submodels.size() > 2){ @@ -485,6 +522,7 @@ void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, b show_sorted(); mod = model; res = modeldatabase->search(model,3); + if(show_search){showModels(res);} printf("results found: %i\n",int(res.size())); fr_res.resize(res.size()); @@ -545,9 +583,9 @@ show_sorted(); if(ud.deleted_models.size() > 0){changed = true; break;} } }else{ - printf("TIME TO ITERATE THROUGH RESULTS\n"); +// printf("TIME TO ITERATE THROUGH RESULTS\n"); for(unsigned int i = 0; i < res.size(); i++){ - printf("TESTING %i\n",i); +// printf("TESTING %i\n",i); reglib::Model * model2 = res[i]; reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model2, reg); @@ -564,10 +602,10 @@ show_sorted(); if(fr.score > 100){ reglib::UpdatedModels ud = mu->fuseData(&fr, model2, model); - printf("merge %i to %i\n",int(model->id),int(model2->id)); - printf("new_models: %i\n",int(ud.new_models.size())); - printf("updated_models: %i\n",int(ud.updated_models.size())); - printf("deleted_models: %i\n",int(ud.deleted_models.size())); +// printf("merge %i to %i\n",int(model->id),int(model2->id)); +// printf("new_models: %i\n",int(ud.new_models.size())); +// printf("updated_models: %i\n",int(ud.updated_models.size())); +// printf("deleted_models: %i\n",int(ud.deleted_models.size())); for(unsigned int j = 0; j < ud.new_models.size(); j++){ new_models[ud.new_models[j]->id] = ud.new_models[j];} for(unsigned int j = 0; j < ud.updated_models.size(); j++){ updated_models[ud.updated_models[j]->id] = ud.updated_models[j];} @@ -580,7 +618,7 @@ show_sorted(); changed = true; delete mu; delete reg; - printf("DONE TESTING %i\n",i); + //printf("DONE TESTING %i\n",i); break; } } @@ -588,7 +626,7 @@ show_sorted(); delete mu; delete reg; - printf("DONE TESTING %i\n",i); + //printf("DONE TESTING %i\n",i); } } @@ -617,7 +655,7 @@ show_sorted(); addToDB(database, it->second); } - printf("end of addToDB: %i %i",int(add),int(deleteIfFail)); + //printf("end of addToDB: %i %i",int(add),int(deleteIfFail)); if(deleteIfFail){ if(!changed){ printf("didnt manage to integrate searchresult\n"); @@ -638,7 +676,7 @@ show_sorted(); void addNewModel(reglib::Model * model){ - printf("model: %i\n",model->frames.size()); + //printf("model: %i\n",model->frames.size()); reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); @@ -652,9 +690,9 @@ void addNewModel(reglib::Model * model){ mu->show_scoring = show_scoring;//fuse scoring show reg->visualizationLvl = show_reg_lvl; - model->print(); + //model->print(); mu->makeInitialSetup(); - model->print(); + //model->print(); delete mu; delete reg; @@ -1053,9 +1091,9 @@ bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_ mu->show_scoring = show_scoring;//fuse scoring show reg->visualizationLvl = show_reg_lvl; -newmodel->print(); +//newmodel->print(); mu->makeInitialSetup(); -newmodel->print(); +//newmodel->print(); delete mu; delete reg; @@ -1584,6 +1622,7 @@ int main(int argc, char **argv){ else if(std::string(argv[i]).compare("-v_db") == 0){ printf("visualization db turned on\n"); visualization = true; show_db = true;} else if(std::string(argv[i]).compare("-intopic") == 0){ printf("intopic input state\n"); inputstate = 11;} else if(std::string(argv[i]).compare("-mdb") == 0){ printf("intopic input state\n"); inputstate = 12;} + else if(std::string(argv[i]).compare("-show_search") == 0){ printf("show_search\n"); show_search = true;} else if(inputstate == 1){ reglib::Camera * cam = reglib::Camera::load(std::string(argv[i])); delete cameras[0]; diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index 20a80e12..5ade690a 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -20,6 +20,17 @@ #include "ceres/rotation.h" #include "ceres/iteration_callback.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + using ceres::NumericDiffCostFunction; using ceres::SizedCostFunction; using ceres::CENTRAL; @@ -30,10 +41,17 @@ using ceres::Solver; using ceres::Solve; using ceres::Solve; +typedef boost::property edge_weight_property; +typedef boost::property vertex_name_property; +using Graph = boost::adjacency_list; +using Vertex = boost::graph_traits::vertex_descriptor; +using VertexIndex = boost::graph_traits::vertices_size_type; +using Edge = boost::graph_traits::edge_descriptor; +using Components = boost::component_index; + namespace reglib { - /** * @brief Returns homogenous 4x4 transformation matrix for given rotation (quaternion) and translation components * @param q rotation represented as quaternion @@ -82,60 +100,6 @@ Mat4f2RotTrans(const Eigen::Matrix4f &tf, Eigen::Quaternionf &q, Eigen::Vector4f trans = tf.block<4,1>(0,3); } -//namespace nanoflann { -// /// KD-tree adaptor for working with data directly stored in an Eigen Matrix, without duplicating the data storage. -// /// This code is adapted from the KDTreeEigenMatrixAdaptor class of nanoflann.hpp -// template -// struct KDTreeAdaptor { -// typedef KDTreeAdaptor self_t; -// typedef typename MatrixType::Scalar num_t; -// typedef typename Distance::template traits::distance_t metric_t; -// typedef KDTreeSingleIndexAdaptor< metric_t,self_t,DIM,IndexType> index_t; -// index_t* index; -// KDTreeAdaptor(const MatrixType &mat, const int leaf_max_size = 10) : m_data_matrix(mat) { -// const size_t dims = mat.rows(); -// index = new index_t( dims, *this, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) ); -// index->buildIndex(); -// } -// ~KDTreeAdaptor() {delete index;} -// const MatrixType &m_data_matrix; -// /// Query for the num_closest closest points to a given point (entered as query_point[0:dim-1]). -// inline void query(const num_t *query_point, const size_t num_closest, IndexType *out_indices, num_t *out_distances_sq) const { -// nanoflann::KNNResultSet resultSet(num_closest); -// resultSet.init(out_indices, out_distances_sq); -// index->findNeighbors(resultSet, query_point, nanoflann::SearchParams()); -// } -// /// Query for the closest points to a given point (entered as query_point[0:dim-1]). -// inline IndexType closest(const num_t *query_point) const { -// IndexType out_indices; -// num_t out_distances_sq; -// query(query_point, 1, &out_indices, &out_distances_sq); -// return out_indices; -// } -// const self_t & derived() const {return *this;} -// self_t & derived() {return *this;} -// inline size_t kdtree_get_point_count() const {return m_data_matrix.cols();} -// /// Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class: -// inline num_t kdtree_distance(const num_t *p1, const size_t idx_p2,size_t size) const { -// num_t s=0; -// for (size_t i=0; i bool kdtree_get_bbox(BBOX&) const {return false;} -// }; -//} - -// This is an exampleof a custom data set class - - template struct ArrayData3D { int rows; T * data; @@ -187,6 +151,7 @@ template struct ArrayData3D { //typedef nanoflann2::KDTreeEigenMatrixAdaptor< Eigen::Matrix, SAMPLES_DIM,nanoflann2::metric_L2_Simple> KDTreed; //typedef nanoflann2::KDTreeEigenMatrixAdaptor< Eigen::Matrix, SAMPLES_DIM,nanoflann2::metric_L2_Simple> KDTreef; double getTime(); + double mysign(double v); diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 78eaf117..9adc229a 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -25,7 +25,7 @@ #include #include #include -#include +//#include #include #include #include @@ -41,6 +41,23 @@ namespace reglib using namespace std; using namespace Eigen; using namespace cv; + + + class ReprojectionResult{ + public: + unsigned long src_ind; + unsigned long dst_ind; + double residual; + double angle; + + ReprojectionResult(unsigned long si, unsigned long di, double r, double a){ + src_ind = si; + dst_ind = di; + residual = r; + angle = a; + } + }; + class UpdatedModels{ public: std::vector< Model * > new_models; diff --git a/quasimodo/quasimodo_models/src/core/Camera.cpp b/quasimodo/quasimodo_models/src/core/Camera.cpp index 31aad614..64b3d974 100644 --- a/quasimodo/quasimodo_models/src/core/Camera.cpp +++ b/quasimodo/quasimodo_models/src/core/Camera.cpp @@ -7,8 +7,6 @@ namespace reglib unsigned int camera_id_count = 0; Camera::Camera(){ - printf("new camera: %ld\n",this); - id = camera_id_count++; width = 640; diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index d916442d..a08db7a6 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -2102,7 +2102,7 @@ pcl::PointCloud::Ptr RGBDFrame::getPCLcloud(){ } void RGBDFrame::savePCD(std::string path, Eigen::Matrix4d pose){ - printf("saving pcd: %s\n",path.c_str()); + // printf("saving pcd: %s\n",path.c_str()); unsigned char * rgbdata = (unsigned char *)rgb.data; unsigned short * depthdata = (unsigned short *)depth.data; diff --git a/quasimodo/quasimodo_models/src/core/Util.cpp b/quasimodo/quasimodo_models/src/core/Util.cpp index eed25a70..1d0877ad 100644 --- a/quasimodo/quasimodo_models/src/core/Util.cpp +++ b/quasimodo/quasimodo_models/src/core/Util.cpp @@ -1,6 +1,10 @@ #include "core/Util.h" namespace reglib{ + double mysign(double v){ + if(v < 0){return -1;} + return 1; + } double getTime(){ struct timeval start1; diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 1493474c..c024c7bd 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -84,7 +84,7 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr //bool * isfused = new bool[width*height]; for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} - printf("wh:%i %i\n",width,height); + //printf("wh:%i %i\n",width,height); for(unsigned int ind = 0; ind < spvec.size();ind++){ superpoint & sp = spvec[ind]; diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 7b8594ba..77afb5f7 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1,15 +1,15 @@ #include "modelupdater/ModelUpdater.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +//#include +//#include +//#include +//#include +//#include +//#include +//#include +//#include +//#include +//#include #include #include "opencv2/core/core.hpp" @@ -17,6 +17,8 @@ #include "opencv2/highgui/highgui.hpp" #include "opencv2/calib3d/calib3d.hpp" +#include "core/Util.h" + namespace gc { @@ -25,30 +27,19 @@ namespace gc } //#include "opencv2/nonfree/nonfree.hpp" -typedef boost::property edge_weight_property; -typedef boost::property vertex_name_property; -using Graph = boost::adjacency_list; -using Vertex = boost::graph_traits::vertex_descriptor; -using VertexIndex = boost::graph_traits::vertices_size_type; -using Edge = boost::graph_traits::edge_descriptor; -using Components = boost::component_index; +//typedef boost::property edge_weight_property; +//typedef boost::property vertex_name_property; +//using Graph = boost::adjacency_list; +//using Vertex = boost::graph_traits::vertex_descriptor; +//using VertexIndex = boost::graph_traits::vertices_size_type; +//using Edge = boost::graph_traits::edge_descriptor; +//using Components = boost::component_index; using namespace std; namespace reglib { -double mysign(double v){ - if(v < 0){return -1;} - return 1; -} - -double getTime(){ - struct timeval start1; - gettimeofday(&start1, NULL); - return double(start1.tv_sec+(start1.tv_usec/1000000.0)); -} - float graph_cut(vector& graphs_out,vector>& second_graphinds, Graph& graph_in, std::vector graph_inds){ using adjacency_iterator = boost::graph_traits::adjacency_iterator; typename boost::property_map::type vertex_id = boost::get(boost::vertex_index, graph_in); @@ -231,6 +222,98 @@ bool ModelUpdater::isRefinementNeeded(){ return failed; } +//OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf){ +// unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); +// unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); +// float * dst_normalsdata = (float *)(cf->normals.data); + +// float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); +// float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); +// float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); + +// Camera * dst_camera = cf->camera; +// const unsigned int dst_width = dst_camera->width; +// const unsigned int dst_height = dst_camera->height; +// const float dst_idepth = dst_camera->idepth_scale; +// const float dst_cx = dst_camera->cx; +// const float dst_cy = dst_camera->cy; +// const float dst_fx = dst_camera->fx; +// const float dst_fy = dst_camera->fy; +// const float dst_ifx = 1.0/dst_camera->fx; +// const float dst_ify = 1.0/dst_camera->fy; +// const unsigned int dst_width2 = dst_camera->width - 2; +// const unsigned int dst_height2 = dst_camera->height - 2; + + +// std::vector src_inds; +// std::vector dst_inds; +// std::vector residuals; +// std::vector cameraSurfaceAngles; +// std::vector angles; +// src_inds.reserve(nr_data); +// dst_inds.reserve(nr_data); +// residuals.reserve(nr_data); +// cameraSurfaceAngles.reserve(nr_data); +// angles.reserve(nr_data); + +// unsigned long nr_data = spvec.size(); +// for(unsigned long ind = 0; ind < nr_data;ind++){ +// superpoint & sp = spvec[ind]; + +// float src_x = sp.point(0); +// float src_y = sp.point(1); +// float src_z = sp.point(2); + +// float src_nx = sp.normal(0); +// float src_ny = sp.normal(1); +// float src_nz = sp.normal(2); + +// float point_information = sp.point_information; + +// float tx = m00*src_x + m01*src_y + m02*src_z + m03; +// float ty = m10*src_x + m11*src_y + m12*src_z + m13; +// float tz = m20*src_x + m21*src_y + m22*src_z + m23; + +// float itz = 1.0/tz; +// float dst_w = dst_fx*tx*itz + dst_cx; +// float dst_h = dst_fy*ty*itz + dst_cy; + +// if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ +// unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + +// float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); +// float dst_nx = dst_normalsdata[3*dst_ind+0]; +// if(dst_z > 0 && dst_nx != 2){ +// if(dst_detdata[dst_ind] != 0){continue;} +// float dst_ny = dst_normalsdata[3*dst_ind+1]; +// float dst_nz = dst_normalsdata[3*dst_ind+2]; + +// float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; +// float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + +// float tnx = m00*src_nx + m01*src_ny + m02*src_nz; +// float tny = m10*src_nx + m11*src_ny + m12*src_nz; +// float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + +// double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); +// double dst_noise = dst_z * dst_z; +// double point_noise = 1.0/sqrt(point_information); + +// double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); +// d *= compare_mul; + +// double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); +// double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); + +// residuals.push_back(d); +// weights.push_back(angle_dst*angle_dst*angle_dst); +// src_inds.push_back(ind); +// dst_inds.push_back(dst_ind); +// } +// } +// } +//} + OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step, bool debugg){ // printf("start:: %s::%i\n",__FILE__,__LINE__); OcclusionScore oc; @@ -639,7 +722,7 @@ void ModelUpdater::makeInitialSetup(){ model->points = getSuperPoints(model->relativeposes,model->frames,model->modelmasks,1,false); - printf("getSuperPoints done\n"); + //printf("getSuperPoints done\n"); vector cp; vector cf; vector cm; @@ -655,7 +738,7 @@ void ModelUpdater::makeInitialSetup(){ model->rep_frames = cf; model->rep_modelmasks = cm; - printf("model->rep_relativeposes: %i\n",model->rep_relativeposes.size()); + //printf("model->rep_relativeposes: %i\n",model->rep_relativeposes.size()); } void ModelUpdater::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, int type, bool debugg){ @@ -693,7 +776,7 @@ void ModelUpdater::addSuperPoints(vector & spvec, Matrix4d p, RGBDFr //bool * isfused = new bool[width*height]; std::vector isfused; - printf("wh: %i %i -> %i\n",width,height,width*height); + //printf("wh: %i %i -> %i\n",width,height,width*height); isfused.resize(width*height); for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} @@ -1019,7 +1102,6 @@ void ModelUpdater::getGoodCompareFrames(vector & cp, vector %f\n",i,score); if(score < worst){ worst = score; worst_id = i; @@ -1036,7 +1118,6 @@ void ModelUpdater::getGoodCompareFrames(vector & cp, vectorgetCD(model2->points.size()/step2); registration->setSrc(cd2); -printf("register_setup_start: %5.5f\n",getTime()-register_setup_start); +//printf("register_setup_start: %5.5f\n",getTime()-register_setup_start); double register_compute_start = getTime(); FusionResults fr = registration->getTransform(guess); -printf("register_compute_start: %5.5f\n",getTime()-register_compute_start); +//printf("register_compute_start: %5.5f\n",getTime()-register_compute_start); double best = -99999999999999; int best_id = -1; double register_evaluate_start = getTime(); @@ -59,7 +59,7 @@ addModelsToVector(testmodels,testrps,model2,Eigen::Matrix4d::Identity()); int todo = fr.candidates.size(); double expectedCost = double(todo)*computeOcclusionScoreCosts(testmodels); -printf("expectedCost: %f\n",expectedCost); +//printf("expectedCost: %f\n",expectedCost); int step = 0.5 + expectedCost/11509168.5;// ~1 sec predicted @@ -107,7 +107,7 @@ printf("expectedCost: %f\n",expectedCost); } double improvement = sumscore2-sumscore1; - printf("improvement: %f\n",improvement); + //printf("improvement: %f\n",improvement); if(improvement > best){ computeOcclusionScore(models,rps,step,false); @@ -117,7 +117,7 @@ printf("expectedCost: %f\n",expectedCost); } } -printf("register_evaluate_start: %5.5f\n",getTime()-register_evaluate_start); +//printf("register_evaluate_start: %5.5f\n",getTime()-register_evaluate_start); if(best_id != -1){ fr.score = 9999999; @@ -139,12 +139,12 @@ UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, UpdatedModels retval = UpdatedModels(); Eigen::Matrix4d pose = f->guess; - printf("MODEL1 "); - model1->print(); - printf("MODEL2 "); - model2->print(); - printf("input pose\n"); - std::cout << pose << std::endl << std::endl; +// printf("MODEL1 "); +// model1->print(); +// printf("MODEL2 "); +// model2->print(); +// printf("input pose\n"); +// std::cout << pose << std::endl << std::endl; // std::vector current_poses; // std::vector current_frames; @@ -169,7 +169,7 @@ UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, addModelsToVector(models,rps,model2,pose); double expectedCost = computeOcclusionScoreCosts(models); - printf("expectedCost: %f\n",expectedCost); +// printf("expectedCost: %f\n",expectedCost); int step = 0.5 + expectedCost/(10.0*11509168.5);// ~10 sec predicted max time step = std::max(1,step); @@ -177,14 +177,14 @@ UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, std::vector > scores = getScores(ocs); std::vector partition = getPartition(scores,2,5,2); - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - if(scores[i][j] >= 0){printf(" ");} - printf("%5.5f ",0.00001*scores[i][j]); - } - printf("\n"); - } - printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); +// for(unsigned int i = 0; i < scores.size(); i++){ +// for(unsigned int j = 0; j < scores.size(); j++){ +// if(scores[i][j] >= 0){printf(" ");} +// printf("%5.5f ",0.00001*scores[i][j]); +// } +// printf("\n"); +// } +// printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); double sumscore1 = 0; @@ -204,17 +204,17 @@ UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, double improvement = sumscore2-sumscore1; - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - if(scores[i][j] > 0){printf(" ");} - printf("%5.5f ",0.0001*scores[i][j]); - } - printf("\n"); - } - printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); - printf("sumscore before part: %f\n",sumscore1); - printf("sumscore after part: %f\n",sumscore2); - printf("improvement: %f\n",improvement); +// for(unsigned int i = 0; i < scores.size(); i++){ +// for(unsigned int j = 0; j < scores.size(); j++){ +// if(scores[i][j] > 0){printf(" ");} +// printf("%5.5f ",0.0001*scores[i][j]); +// } +// printf("\n"); +// } +// printf("partition "); for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); +// printf("sumscore before part: %f\n",sumscore1); +// printf("sumscore after part: %f\n",sumscore2); +// printf("improvement: %f\n",improvement); //exit(0); std::vector count; for(unsigned int i = 0; i < partition.size(); i++){ diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index f68e544a..f1316ee3 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -1420,7 +1420,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ } } - printf("depthedge_count: %i\n",depthedge_count); + //printf("depthedge_count: %i\n",depthedge_count); if(depthedge_count > 10){ double * depthedge_ap = new double[3*depthedge_count]; double * depthedge_ai = new double[3*depthedge_count]; @@ -1475,7 +1475,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ // } } - printf("total load time: %5.5f\n",getTime()-total_load_time_start); + //printf("total load time: %5.5f\n",getTime()-total_load_time_start); } void MassRegistrationPPR2::addData(pcl::PointCloud::Ptr cloud){ @@ -3270,7 +3270,7 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectorregularization = std::max(reg1,reg2); kpfunc->regularization = std::max(reg1,reg2); - printf("func->noiseval: %f func->regularization: %f\n",func->noiseval,func->regularization); +// printf("func->noiseval: %f func->regularization: %f\n",func->noiseval,func->regularization); //printf("ratio: %f kpratio: %f\n",ratio,kpratio); //if(func->noiseval >= kpfunc->noiseval && 0.01*func->noiseval < func->regularization){break;} //if(func->noiseval < kpfunc->noiseval && 0.01*kpfunc->noiseval < kpfunc->regularization){break;} @@ -3278,11 +3278,12 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectornoiseval > 10.0*func->regularization && ratio > 0.75){break;} } - printf("total_time: %5.5f\n",getTime()-total_time_start); - printf("rematch_time: %5.5f\n",rematch_time); - printf("compM residuals_time:%5.5f\n",residuals_time); - printf("computeModel: %5.5f\n",computeModel_time); - printf("opt_time: %5.5f\n",opt_time); +// printf("total_time: %5.5f\n",getTime()-total_time_start); +// printf("rematch_time: %5.5f\n",rematch_time); +// printf("compM residuals_time:%5.5f\n",residuals_time); +// printf("computeModel: %5.5f\n",computeModel_time); +// printf("opt_time: %5.5f\n",opt_time); + // printf("setup_matches_time: %5.5f\n",setup_matches_time); // printf("setup_equation_time: %5.5f\n",setup_equation_time); // printf("solve_equation_time: %5.5f\n",solve_equation_time); @@ -3299,7 +3300,7 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector guess)\n"); +// printf("stop MassRegistrationPPR2::getTransforms(std::vector guess)\n"); //exit(0); return MassFusionResults(poses,-1); } From cf836205ca06f79fc9901089392fd6da6bf49ef7 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 7 Sep 2016 22:58:59 +0200 Subject: [PATCH 098/255] ... --- .../quasimodo_models/include/core/Util.h | 42 +++ .../include/modelupdater/ModelUpdater.h | 60 ++--- quasimodo/quasimodo_models/src/core/Util.cpp | 198 ++++++++++++++ .../src/modelupdater/ModelUpdater.cpp | 244 +----------------- 4 files changed, 274 insertions(+), 270 deletions(-) diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index 5ade690a..4a4f6ec6 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -31,6 +31,9 @@ #include #include +#include +#include + using ceres::NumericDiffCostFunction; using ceres::SizedCostFunction; using ceres::CENTRAL; @@ -52,6 +55,45 @@ using Components = boost::component_index; namespace reglib { + +class ReprojectionResult{ + public: + unsigned long src_ind; + unsigned long dst_ind; + double residual; + double angle; + + ReprojectionResult(unsigned long si, unsigned long di, double r, double a){ + src_ind = si; + dst_ind = di; + residual = r; + angle = a; + } +}; + +class OcclusionScore{ + public: + double score; + double occlusions; + + OcclusionScore(){score = 0;occlusions = 0;} + OcclusionScore( double score_ ,double occlusions_){score = score_;occlusions = occlusions_;} + ~OcclusionScore(){} + + void add(OcclusionScore oc){ + score += oc.score; + occlusions += oc.occlusions; + } + + void print(){printf("score: %5.5f occlusions: %5.5f\n",score,occlusions);} +}; + +float graph_cut(std::vector& graphs_out,std::vector>& second_graphinds, Graph& graph_in, std::vector graph_inds); + +float recursive_split(std::vector * graphs_out,std::vector> * graphinds_out, Graph * graph, std::vector graph_inds); + +std::vector partition_graph(std::vector< std::vector< float > > & scores); + /** * @brief Returns homogenous 4x4 transformation matrix for given rotation (quaternion) and translation components * @param q rotation represented as quaternion diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 9adc229a..27d0c6e3 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -43,20 +43,20 @@ namespace reglib using namespace cv; - class ReprojectionResult{ - public: - unsigned long src_ind; - unsigned long dst_ind; - double residual; - double angle; - - ReprojectionResult(unsigned long si, unsigned long di, double r, double a){ - src_ind = si; - dst_ind = di; - residual = r; - angle = a; - } - }; +// class ReprojectionResult{ +// public: +// unsigned long src_ind; +// unsigned long dst_ind; +// double residual; +// double angle; + +// ReprojectionResult(unsigned long si, unsigned long di, double r, double a){ +// src_ind = si; +// dst_ind = di; +// residual = r; +// angle = a; +// } +// }; class UpdatedModels{ public: @@ -68,22 +68,22 @@ namespace reglib UpdatedModels(){} }; - class OcclusionScore{ - public: - double score; - double occlusions; - - OcclusionScore(){score = 0;occlusions = 0;} - OcclusionScore( double score_ ,double occlusions_){score = score_;occlusions = occlusions_;} - ~OcclusionScore(){} - - void add(OcclusionScore oc){ - score += oc.score; - occlusions += oc.occlusions; - } - - void print(){printf("score: %5.5f occlusions: %5.5f\n",score,occlusions);} - }; +// class OcclusionScore{ +// public: +// double score; +// double occlusions; + +// OcclusionScore(){score = 0;occlusions = 0;} +// OcclusionScore( double score_ ,double occlusions_){score = score_;occlusions = occlusions_;} +// ~OcclusionScore(){} + +// void add(OcclusionScore oc){ +// score += oc.score; +// occlusions += oc.occlusions; +// } + +// void print(){printf("score: %5.5f occlusions: %5.5f\n",score,occlusions);} +// }; class ModelUpdater{ public: diff --git a/quasimodo/quasimodo_models/src/core/Util.cpp b/quasimodo/quasimodo_models/src/core/Util.cpp index 1d0877ad..da4c486e 100644 --- a/quasimodo/quasimodo_models/src/core/Util.cpp +++ b/quasimodo/quasimodo_models/src/core/Util.cpp @@ -12,6 +12,112 @@ namespace reglib{ return double(start1.tv_sec+(start1.tv_usec/1000000.0)); } + float graph_cut(std::vector& graphs_out,std::vector>& second_graphinds, Graph& graph_in, std::vector graph_inds){ + using adjacency_iterator = boost::graph_traits::adjacency_iterator; + typename boost::property_map::type vertex_id = boost::get(boost::vertex_index, graph_in); + typename boost::property_map::type edge_id = boost::get(boost::edge_weight, graph_in); + typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, graph_in); + + BOOST_AUTO(parities, boost::make_one_bit_color_map(boost::num_vertices(graph_in), boost::get(boost::vertex_index, graph_in))); + + float w = boost::stoer_wagner_min_cut(graph_in, boost::get(boost::edge_weight, graph_in), boost::parity_map(parities)); + + std::unordered_map mappings; + VertexIndex counters[2] = {0, 0}; + + graphs_out.push_back(new Graph(1)); + graphs_out.push_back(new Graph(1)); + second_graphinds.push_back(std::vector()); + second_graphinds.push_back(std::vector()); + //std::cout << "One set of vertices consists of:" << std::endl; + bool flag; + Edge edge; + for (size_t i = 0; i < boost::num_vertices(graph_in); ++i) { + int first = boost::get(parities, i); + second_graphinds[first].push_back(graph_inds[i]); + // iterate adjacent edges + adjacency_iterator ai, ai_end; + for (tie(ai, ai_end) = boost::adjacent_vertices(i, graph_in); ai != ai_end; ++ai) { + VertexIndex neighbor_index = boost::get(vertex_id, *ai); + int second = boost::get(parities, neighbor_index); + if (first == second && neighbor_index < i) { + tie(edge, flag) = boost::edge(i, neighbor_index, graph_in); + edge_weight_property weight = boost::get(edge_id, edge); + if (mappings.count(i) == 0) { + mappings[i] = counters[first]++; + } + if (mappings.count(neighbor_index) == 0) { + mappings[neighbor_index] = counters[first]++; + } + tie(edge, flag) = boost::add_edge(mappings[neighbor_index], mappings[i], weight, *graphs_out[first]); + + typename boost::property_map::type vertex_name_first = boost::get(boost::vertex_name, *graphs_out[first]); + boost::get(vertex_name_first, mappings[i]) = boost::get(vertex_name, i); + boost::get(vertex_name_first, mappings[neighbor_index]) = boost::get(vertex_name, *ai); + } + } + } + return w; + } + + float recursive_split(std::vector * graphs_out,std::vector> * graphinds_out, Graph * graph, std::vector graph_inds){ + if(boost::num_vertices(*graph) == 1){ + graphs_out->push_back(graph); + graphinds_out->push_back(graph_inds); + return 0; + } + + std::vector second_graphs; + std::vector> second_graphinds; + float w = graph_cut(second_graphs,second_graphinds,*graph,graph_inds); + if(w <= 0){ + delete graph; + return 2*w + recursive_split(graphs_out, graphinds_out,second_graphs.front(),second_graphinds.front()) + recursive_split(graphs_out, graphinds_out, second_graphs.back(),second_graphinds.back()); + }else{ + graphs_out->push_back(graph); + graphinds_out->push_back(graph_inds); + delete second_graphs.front(); + delete second_graphs.back(); + return 0; + } + } + + std::vector partition_graph(std::vector< std::vector< float > > & scores){ + int nr_data = scores.size(); + Graph* graph = new Graph(nr_data); + std::vector graph_inds; + graph_inds.resize(nr_data); + + typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, *graph); + + float sum = 0; + for(int i = 0; i < nr_data; i++){ + graph_inds[i] = i; + for(int j = i+1; j < nr_data; j++){ + float weight = scores[i][j]; + if(weight != 0){ + sum += 2*weight; + edge_weight_property e = weight; + boost::add_edge(i, j, e, *graph); + } + } + } + + std::vector * graphs_out = new std::vector(); + std::vector> * graphinds_out = new std::vector>(); + float best = sum-recursive_split(graphs_out,graphinds_out, graph,graph_inds ); + + std::vector part; + part.resize(nr_data); + //int * part = new int[nr_data]; + for(unsigned int i = 0; i < graphinds_out->size(); i++){ + for(unsigned int j = 0; j < graphinds_out->at(i).size(); j++){ + part[graphinds_out->at(i).at(j)] = i; + } + } + return part; + } + Eigen::Matrix4d getMatTest(const double * const camera, int mode){ Eigen::Matrix4d ret = Eigen::Matrix4d::Identity(); double rr [9]; @@ -203,4 +309,96 @@ namespace reglib{ if(change_trans < stopvalt && change_rot < stopvalr){return true;} else{return false;} } + + //OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf){ + // unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); + // unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); + // float * dst_normalsdata = (float *)(cf->normals.data); + + // float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); + // float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); + // float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); + + // Camera * dst_camera = cf->camera; + // const unsigned int dst_width = dst_camera->width; + // const unsigned int dst_height = dst_camera->height; + // const float dst_idepth = dst_camera->idepth_scale; + // const float dst_cx = dst_camera->cx; + // const float dst_cy = dst_camera->cy; + // const float dst_fx = dst_camera->fx; + // const float dst_fy = dst_camera->fy; + // const float dst_ifx = 1.0/dst_camera->fx; + // const float dst_ify = 1.0/dst_camera->fy; + // const unsigned int dst_width2 = dst_camera->width - 2; + // const unsigned int dst_height2 = dst_camera->height - 2; + + + // std::vector src_inds; + // std::vector dst_inds; + // std::vector residuals; + // std::vector cameraSurfaceAngles; + // std::vector angles; + // src_inds.reserve(nr_data); + // dst_inds.reserve(nr_data); + // residuals.reserve(nr_data); + // cameraSurfaceAngles.reserve(nr_data); + // angles.reserve(nr_data); + + // unsigned long nr_data = spvec.size(); + // for(unsigned long ind = 0; ind < nr_data;ind++){ + // superpoint & sp = spvec[ind]; + + // float src_x = sp.point(0); + // float src_y = sp.point(1); + // float src_z = sp.point(2); + + // float src_nx = sp.normal(0); + // float src_ny = sp.normal(1); + // float src_nz = sp.normal(2); + + // float point_information = sp.point_information; + + // float tx = m00*src_x + m01*src_y + m02*src_z + m03; + // float ty = m10*src_x + m11*src_y + m12*src_z + m13; + // float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + // float itz = 1.0/tz; + // float dst_w = dst_fx*tx*itz + dst_cx; + // float dst_h = dst_fy*ty*itz + dst_cy; + + // if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + // unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + + // float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + // float dst_nx = dst_normalsdata[3*dst_ind+0]; + // if(dst_z > 0 && dst_nx != 2){ + // if(dst_detdata[dst_ind] != 0){continue;} + // float dst_ny = dst_normalsdata[3*dst_ind+1]; + // float dst_nz = dst_normalsdata[3*dst_ind+2]; + + // float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + // float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + // float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + // float tny = m10*src_nx + m11*src_ny + m12*src_nz; + // float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + // double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + // double dst_noise = dst_z * dst_z; + // double point_noise = 1.0/sqrt(point_information); + + // double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); + // d *= compare_mul; + + // double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); + // double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); + + // residuals.push_back(d); + // weights.push_back(angle_dst*angle_dst*angle_dst); + // src_inds.push_back(ind); + // dst_inds.push_back(dst_ind); + // } + // } + // } + //} } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 77afb5f7..8aaa2cbc 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -25,130 +25,13 @@ namespace gc #include "graphcuts/graph.cpp" #include "graphcuts/maxflow.cpp" } -//#include "opencv2/nonfree/nonfree.hpp" - -//typedef boost::property edge_weight_property; -//typedef boost::property vertex_name_property; -//using Graph = boost::adjacency_list; -//using Vertex = boost::graph_traits::vertex_descriptor; -//using VertexIndex = boost::graph_traits::vertices_size_type; -//using Edge = boost::graph_traits::edge_descriptor; -//using Components = boost::component_index; using namespace std; namespace reglib { -float graph_cut(vector& graphs_out,vector>& second_graphinds, Graph& graph_in, std::vector graph_inds){ - using adjacency_iterator = boost::graph_traits::adjacency_iterator; - typename boost::property_map::type vertex_id = boost::get(boost::vertex_index, graph_in); - typename boost::property_map::type edge_id = boost::get(boost::edge_weight, graph_in); - typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, graph_in); - - BOOST_AUTO(parities, boost::make_one_bit_color_map(boost::num_vertices(graph_in), boost::get(boost::vertex_index, graph_in))); - - float w = boost::stoer_wagner_min_cut(graph_in, boost::get(boost::edge_weight, graph_in), boost::parity_map(parities)); - - unordered_map mappings; - VertexIndex counters[2] = {0, 0}; - - graphs_out.push_back(new Graph(1)); - graphs_out.push_back(new Graph(1)); - second_graphinds.push_back(vector()); - second_graphinds.push_back(vector()); - //std::cout << "One set of vertices consists of:" << std::endl; - bool flag; - Edge edge; - for (size_t i = 0; i < boost::num_vertices(graph_in); ++i) { - int first = boost::get(parities, i); - second_graphinds[first].push_back(graph_inds[i]); - // iterate adjacent edges - adjacency_iterator ai, ai_end; - for (tie(ai, ai_end) = boost::adjacent_vertices(i, graph_in); ai != ai_end; ++ai) { - VertexIndex neighbor_index = boost::get(vertex_id, *ai); - int second = boost::get(parities, neighbor_index); - if (first == second && neighbor_index < i) { - tie(edge, flag) = boost::edge(i, neighbor_index, graph_in); - edge_weight_property weight = boost::get(edge_id, edge); - if (mappings.count(i) == 0) { - mappings[i] = counters[first]++; - } - if (mappings.count(neighbor_index) == 0) { - mappings[neighbor_index] = counters[first]++; - } - tie(edge, flag) = boost::add_edge(mappings[neighbor_index], mappings[i], weight, *graphs_out[first]); - - typename boost::property_map::type vertex_name_first = boost::get(boost::vertex_name, *graphs_out[first]); - boost::get(vertex_name_first, mappings[i]) = boost::get(vertex_name, i); - boost::get(vertex_name_first, mappings[neighbor_index]) = boost::get(vertex_name, *ai); - } - } - } - return w; -} - -float recursive_split(std::vector * graphs_out,std::vector> * graphinds_out, Graph * graph, std::vector graph_inds){ - if(boost::num_vertices(*graph) == 1){ - graphs_out->push_back(graph); - graphinds_out->push_back(graph_inds); - return 0; - } - - vector second_graphs; - vector> second_graphinds; - float w = graph_cut(second_graphs,second_graphinds,*graph,graph_inds); - if(w <= 0){ - delete graph; - return 2*w + recursive_split(graphs_out, graphinds_out,second_graphs.front(),second_graphinds.front()) + recursive_split(graphs_out, graphinds_out, second_graphs.back(),second_graphinds.back()); - }else{ - graphs_out->push_back(graph); - graphinds_out->push_back(graph_inds); - delete second_graphs.front(); - delete second_graphs.back(); - return 0; - } -} - -std::vector partition_graph(std::vector< std::vector< float > > & scores){ - int nr_data = scores.size(); - Graph* graph = new Graph(nr_data); - std::vector graph_inds; - graph_inds.resize(nr_data); - - typename boost::property_map::type vertex_name = boost::get(boost::vertex_name, *graph); - - float sum = 0; - for(int i = 0; i < nr_data; i++){ - graph_inds[i] = i; - for(int j = i+1; j < nr_data; j++){ - float weight = scores[i][j]; - if(weight != 0){ - sum += 2*weight; - edge_weight_property e = weight; - boost::add_edge(i, j, e, *graph); - } - } - } - - std::vector * graphs_out = new std::vector(); - std::vector> * graphinds_out = new std::vector>(); - float best = sum-recursive_split(graphs_out,graphinds_out, graph,graph_inds ); - - std::vector part; - part.resize(nr_data); - //int * part = new int[nr_data]; - for(unsigned int i = 0; i < graphinds_out->size(); i++){ - for(unsigned int j = 0; j < graphinds_out->at(i).size(); j++){ - part[graphinds_out->at(i).at(j)] = i; - } - } - return part; -} - -std::vector ModelUpdater::getPartition(std::vector< std::vector< float > > & scores, int dims, int nr_todo, double timelimit){ - return partition_graph(scores); -} +std::vector ModelUpdater::getPartition(std::vector< std::vector< float > > & scores, int dims, int nr_todo, double timelimit){return partition_graph(scores);} ModelUpdater::ModelUpdater(){ occlusion_penalty = 10; @@ -174,7 +57,6 @@ FusionResults ModelUpdater::registerModel(Model * model2, Eigen::Matrix4d guess, void ModelUpdater::fuse(Model * model2, Eigen::Matrix4d guess, double uncertanity){ for(unsigned int i = 0; i < model2->frames.size();i++){ model->frames.push_back(model2->frames[i]); - //model->masks.push_back(model2->masks[i]); model->modelmasks.push_back(model2->modelmasks[i]); model->relativeposes.push_back(guess*model2->relativeposes[i]); } @@ -186,8 +68,6 @@ bool ModelUpdater::isRefinementNeeded(){ vector > occlusionScores; occlusionScores.resize(model->frames.size()); for(unsigned int i = 0; i < model->frames.size(); i++){occlusionScores[i].resize(model->frames.size());} - - for(unsigned int i = 0; i < model->frames.size(); i++){ for(unsigned int j = i+1; j < model->frames.size(); j++){ Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; @@ -195,127 +75,14 @@ bool ModelUpdater::isRefinementNeeded(){ occlusionScores[i][j] = computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,false); } } - - std::vector > scores = getScores(occlusionScores); std::vector partition = getPartition(scores,2,5,2); - bool failed = false; for(unsigned int i = 0; i < partition.size(); i++){failed = failed | (partition[i]!=0);} - - for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - if(scores[i][j] > 0){printf(" ");} - printf("%5.5f ",0.0001*scores[i][j]); - // if(scores[i][j] < 0){ - // Eigen::Matrix4d relative_pose = model->relativeposes[i].inverse() * model->relativeposes[j]; - // computeOcclusionScore(model->frames[j], model->modelmasks[j],model->frames[i], model->modelmasks[i], relative_pose,1,true); - // computeOcclusionScore(model->frames[i], model->modelmasks[i],model->frames[j], model->modelmasks[j], relative_pose.inverse(),1,true); - // } - } - printf("\n"); - } - - printf("partition"); - for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} - printf("\n"); return failed; } -//OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf){ -// unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); -// unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); -// float * dst_normalsdata = (float *)(cf->normals.data); - -// float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); -// float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); -// float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); - -// Camera * dst_camera = cf->camera; -// const unsigned int dst_width = dst_camera->width; -// const unsigned int dst_height = dst_camera->height; -// const float dst_idepth = dst_camera->idepth_scale; -// const float dst_cx = dst_camera->cx; -// const float dst_cy = dst_camera->cy; -// const float dst_fx = dst_camera->fx; -// const float dst_fy = dst_camera->fy; -// const float dst_ifx = 1.0/dst_camera->fx; -// const float dst_ify = 1.0/dst_camera->fy; -// const unsigned int dst_width2 = dst_camera->width - 2; -// const unsigned int dst_height2 = dst_camera->height - 2; - - -// std::vector src_inds; -// std::vector dst_inds; -// std::vector residuals; -// std::vector cameraSurfaceAngles; -// std::vector angles; -// src_inds.reserve(nr_data); -// dst_inds.reserve(nr_data); -// residuals.reserve(nr_data); -// cameraSurfaceAngles.reserve(nr_data); -// angles.reserve(nr_data); - -// unsigned long nr_data = spvec.size(); -// for(unsigned long ind = 0; ind < nr_data;ind++){ -// superpoint & sp = spvec[ind]; - -// float src_x = sp.point(0); -// float src_y = sp.point(1); -// float src_z = sp.point(2); - -// float src_nx = sp.normal(0); -// float src_ny = sp.normal(1); -// float src_nz = sp.normal(2); - -// float point_information = sp.point_information; - -// float tx = m00*src_x + m01*src_y + m02*src_z + m03; -// float ty = m10*src_x + m11*src_y + m12*src_z + m13; -// float tz = m20*src_x + m21*src_y + m22*src_z + m23; - -// float itz = 1.0/tz; -// float dst_w = dst_fx*tx*itz + dst_cx; -// float dst_h = dst_fy*ty*itz + dst_cy; - -// if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ -// unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - -// float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); -// float dst_nx = dst_normalsdata[3*dst_ind+0]; -// if(dst_z > 0 && dst_nx != 2){ -// if(dst_detdata[dst_ind] != 0){continue;} -// float dst_ny = dst_normalsdata[3*dst_ind+1]; -// float dst_nz = dst_normalsdata[3*dst_ind+2]; - -// float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; -// float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - -// float tnx = m00*src_nx + m01*src_ny + m02*src_nz; -// float tny = m10*src_nx + m11*src_ny + m12*src_nz; -// float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - -// double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); -// double dst_noise = dst_z * dst_z; -// double point_noise = 1.0/sqrt(point_information); - -// double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); -// d *= compare_mul; - -// double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); -// double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); - -// residuals.push_back(d); -// weights.push_back(angle_dst*angle_dst*angle_dst); -// src_inds.push_back(ind); -// dst_inds.push_back(dst_ind); -// } -// } -// } -//} - OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step, bool debugg){ - // printf("start:: %s::%i\n",__FILE__,__LINE__); OcclusionScore oc; unsigned char * dst_maskdata = (unsigned char *)(cm->mask.data); @@ -1123,14 +890,12 @@ void ModelUpdater::getGoodCompareFrames(vector & cp, vector > ocs = computeOcclusionScore(model->submodels,model->submodels_relativeposes,1,false); - printf("%s::%i\n",__FILE__,__LINE__); + std::vector > scores = getScores(ocs); - printf("%s::%i\n",__FILE__,__LINE__); + double sumscore_bef = 0; for(unsigned int i = 0; i < scores.size(); i++){ @@ -1172,7 +937,6 @@ void ModelUpdater::refine(double reg,bool useFullMask, int visualization){ } void ModelUpdater::show(bool stop){ - //printf("void ModelUpdater::show()\n"); pcl::PointCloud::Ptr cloud = model->getPCLcloud(1,false); viewer->removeAllPointClouds(); viewer->addPointCloud(cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); From 79b7d50c3b68f611761f7301db10f5c7b096f892 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 9 Sep 2016 16:56:33 +0200 Subject: [PATCH 099/255] integration improved, modelserver cleaned etc --- quasimodo/quasimodo_brain/src/modelserver.cpp | 1322 ++++++++--------- .../quasimodo_models/include/core/RGBDFrame.h | 2 +- .../quasimodo_models/include/core/Util.h | 43 + .../quasimodo_models/include/model/Model.h | 84 +- .../registration/MassRegistrationPPR2.h | 2 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 96 ++ quasimodo/quasimodo_models/src/core/Util.cpp | 92 -- .../src/modelupdater/ModelUpdater.cpp | 4 +- .../src/registration/MassRegistrationPPR2.cpp | 27 +- 9 files changed, 858 insertions(+), 814 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index b1d0a09a..e2762ffc 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -88,6 +88,10 @@ ros::Publisher model_places_pub; ros::Publisher model_last_pub; ros::Publisher chatter_pub; +ros::ServiceClient retrieval_client; +ros::ServiceClient conversion_client; +ros::ServiceClient insert_client; + ros::ServiceClient soma2add; double occlusion_penalty = 10; @@ -115,7 +119,7 @@ void publishDatabasePCD(bool original_colors = false){ float maxx = 0; for(unsigned int i = 0; i < results.size(); i++){ - pcl::PointCloud::Ptr cloud = results[i]->getPCLcloud(1, false); + pcl::PointCloud::Ptr cloud = results[i]->getPCLcloud(1, false); float meanx = 0; float meany = 0; float meanz = 0; @@ -145,7 +149,7 @@ void publishDatabasePCD(bool original_colors = false){ sensor_msgs::PointCloud2 input; pcl::toROSMsg (*conccloud,input);//, *transformed_cloud); - input.header.frame_id = "/map"; + input.header.frame_id = "/map"; database_pcd_pub.publish(input); } @@ -153,55 +157,26 @@ void publish_history(std::vector::Ptr> history for(unsigned int i = 0; i < history.size(); i++){ sensor_msgs::PointCloud2 input; pcl::toROSMsg (*history[i],input);//, *transformed_cloud); - input.header.frame_id = "/map"; + input.header.frame_id = "/map"; model_history_pub.publish(input); } } void dumpDatabase(std::string path = "."){ - - char command [1024]; - sprintf(command,"rm -r -f %s/database_tmp",path.c_str()); - //printf("%s\n",command); - int r = system(command); - - sprintf(command,"mkdir %s/database_tmp",path.c_str()); - //printf("%s\n",command); - r = system(command); - - for(unsigned int m = 0; m < modeldatabase->models.size(); m++){ - char buf [1024]; - sprintf(buf,"%s/database_tmp/model_%08i",path.c_str(),m); - sprintf(command,"mkdir -p %s",buf); - r = system(command); - - - modeldatabase->models[m]->save(std::string(buf)); - } - - /* char command [1024]; - sprintf(command,"rm -r -f %s/quasimodo_model*",path.c_str()); - printf("%s\n",command); + sprintf(command,"rm -r -f %s/database_tmp",path.c_str()); int r = system(command); - sprintf(command,"rm -f %s/quasimodo_camera*",path.c_str()); - printf("%s\n",command); + sprintf(command,"mkdir %s/database_tmp",path.c_str()); r = system(command); - cameras[0]->save(path+"/quasimodo_camera0"); for(unsigned int m = 0; m < modeldatabase->models.size(); m++){ char buf [1024]; - sprintf(buf,"%s/quasimodo_model%i",path.c_str(),m); + sprintf(buf,"%s/database_tmp/model_%08i",path.c_str(),m); sprintf(command,"mkdir -p %s",buf); r = system(command); - - sprintf(command,"mkdir -p %s/views",buf); - r = system(command); - modeldatabase->models[m]->save(std::string(buf)); } - */ } void retrievalCallback(const quasimodo_msgs::retrieval_query_result & qr){ @@ -249,52 +224,12 @@ void showModels(std::vector mods){ int savecounter = 0; void show_sorted(){ - //printf("show_sorted\n"); - if(!show_db){return;} + if(!show_db){return;} if(!visualization){return;} std::vector results; for(unsigned int i = 0; i < modeldatabase->models.size(); i++){results.push_back(modeldatabase->models[i]);} std::sort (results.begin(), results.end(), myfunction); showModels(results); - -// viewer->removeAllPointClouds(); -// float maxx = 0; - -// printf("pre->loop\n"); -// for(unsigned int i = 0; i < results.size(); i++){ -// pcl::PointCloud::Ptr cloud = results[i]->getPCLcloud(1, false); -// float meanx = 0; -// float meany = 0; -// float meanz = 0; -// for(unsigned int j = 0; j < cloud->points.size(); j++){ -// meanx += cloud->points[j].x; -// meany += cloud->points[j].y; -// meanz += cloud->points[j].z; -// } -// meanx /= float(cloud->points.size()); -// meany /= float(cloud->points.size()); -// meanz /= float(cloud->points.size()); - -// for(unsigned int j = 0; j < cloud->points.size(); j++){ -// cloud->points[j].x -= meanx; -// cloud->points[j].y -= meany; -// cloud->points[j].z -= meanz; -// } - -// float minx = 100000000000; - -// for(unsigned int j = 0; j < cloud->points.size(); j++){minx = std::min(cloud->points[j].x , minx);} -// for(unsigned int j = 0; j < cloud->points.size(); j++){cloud->points[j].x += maxx-minx + 0.15;} -// for(unsigned int j = 0; j < cloud->points.size(); j++){maxx = std::max(cloud->points[j].x,maxx);} -// char buf [1024]; -// sprintf(buf,"cloud%i",i); -// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); -// } -// viewer->spin(); - //char buf [1024]; - //sprintf(buf,"globalimg%i.png",savecounter++); - //viewer->saveScreenshot(std::string(buf)); - //viewer->spinOnce(); } std::vector getSOMA2ObjectMSGs(reglib::Model * model){ @@ -396,18 +331,10 @@ std::vector getSOMA2ObjectMSGs(reglib::Model * model){ } void publishModel(reglib::Model * model){ - // std::vector somamsgs = getSOMA2ObjectMSGs(model); soma_manager::SOMA2InsertObjs objs; objs.request.objects = getSOMA2ObjectMSGs(model); - if (soma2add.call(objs)){//Add frame to model server - //// int frame_id = ifsrv.response.frame_id; + if (soma2add.call(objs)){ }else{ROS_ERROR("Failed to call service soma2add");} - - // for(int i = 0; i < somamsgs.size(); i++){ - // if (soma2add.call(somamsgs[i])){//Add frame to model server - //// int frame_id = ifsrv.response.frame_id; - // }else{ROS_ERROR("Failed to call service soma2add");} - // } } @@ -436,69 +363,69 @@ bool indexFrame(quasimodo_msgs::index_frame::Request & req, quasimodo_msgs::ind Eigen::Affine3d epose; tf::poseMsgToEigen(pose, epose); - reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS - cam->fx = camera.K[0]; - cam->fy = camera.K[4]; - cam->cx = camera.K[2]; - cam->cy = camera.K[5]; + reglib::Camera * cam = new reglib::Camera();//TODO:: ADD TO CAMERAS + cam->fx = camera.K[0]; + cam->fy = camera.K[4]; + cam->cx = camera.K[2]; + cam->cy = camera.K[5]; - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); frames[frame->id] = frame; res.frame_id = frame->id; return true; } +bool addToDB(ModelDatabase * database, reglib::Model * model, bool add);//, bool deleteIfFail = false); +bool addIfPossible(ModelDatabase * database, reglib::Model * model, reglib::Model * model2){ +// std::map new_models; +// std::map updated_models; -class SearchResult{ -public: - double score; - reglib::Model * model; - Eigen::Affine3d pose; - - SearchResult(double score_, reglib::Model * model_, Eigen::Affine3d pose_){ - score = score_; - model = model_; - pose = pose_; - } - ~SearchResult(){} -}; - -bool removeModel(int id){ - //printf("removeModel: %i\n",id); - delete models[id]; - models.erase(id); - delete updaters[id]; - updaters.erase(id); -} - - -reglib::Model * mod; -std::vector res; -std::vector fr_res; - -void call_from_thread(int i) { - reglib::Model * model2 = res[i]; - - - printf("testreg %i to %i\n",int(mod->id),int(model2->id)); - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model2, reg); mu->occlusion_penalty = occlusion_penalty; mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; - reg->visualizationLvl = 0; + mu->show_init_lvl = show_init_lvl;//init show + mu->show_refine_lvl = show_refine_lvl;//refine show + mu->show_scoring = show_scoring;//fuse scoring show + reg->visualizationLvl = show_reg_lvl; + reglib::FusionResults fr = mu->registerModel(model); - reglib::FusionResults fr = mu->registerModel(mod); - fr_res[i] = fr; + if(fr.score > 100){ + reglib::UpdatedModels ud = mu->fuseData(&fr, model2, model); + delete mu; + delete reg; + + if(ud.deleted_models.size() > 0 || ud.updated_models.size() > 0 || ud.new_models.size() > 0){ + for(unsigned int j = 0; j < ud.deleted_models.size(); j++){ + database->remove(ud.deleted_models[j]); + delete ud.deleted_models[j]; + } + + for(unsigned int j = 0; j < ud.updated_models.size(); j++){ + database->remove(ud.updated_models[j]); + models_deleted_pub.publish(getModelMSG(ud.updated_models[j])); + } - delete mu; - delete reg; -} -void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, bool deleteIfFail = false){ - //printf("addToDB %i %i\n",int(add),int(deleteIfFail)); + for(unsigned int j = 0; j < ud.updated_models.size(); j++){ + addToDB(database, ud.updated_models[j], true); + } + + for(unsigned int j = 0; j < ud.new_models.size(); j++){ + addToDB(database, ud.new_models[j], true); + } + return true; + } + }else{ + delete mu; + delete reg; + } + return false; +} +bool addToDB(ModelDatabase * database, reglib::Model * model, bool add){// = true){, bool deleteIfFail = false){ if(add){ if(model->submodels.size() > 2){ reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); @@ -507,178 +434,211 @@ void addToDB(ModelDatabase * database, reglib::Model * model, bool add = true, b mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; reg->visualizationLvl = 0; - mu->show_init_lvl = show_init_lvl;//init show - mu->show_refine_lvl = show_refine_lvl;//refine show - mu->show_scoring = show_scoring;//fuse scoring show + mu->show_init_lvl = show_init_lvl;//init show + mu->show_refine_lvl = show_refine_lvl;//refine show + mu->show_scoring = show_scoring;//fuse scoring show mu->refine(0.001,false,0); delete mu; delete reg; } database->add(model); - //models_new_pub.publish(getModelMSG(model)); model->last_changed = ++current_model_update; - // show_sorted(); } -show_sorted(); - mod = model; - res = modeldatabase->search(model,3); - if(show_search){showModels(res);} - printf("results found: %i\n",int(res.size())); - fr_res.resize(res.size()); + std::vector res = modeldatabase->search(model,3); + if(show_search){showModels(res);} - if(res.size() == 0){ - printf("no candidates found in database!\n"); - return; + for(unsigned int i = 0; i < res.size(); i++){ + if(addIfPossible(database,model,res[i])){return true;} } + return false; +} - bool changed = false; - std::map new_models; - std::map updated_models; - std::vector models2merge; - std::vector fr2merge; - bool multi_thread = false; - if(multi_thread){ - const int num_threads = res.size(); - std::thread t[num_threads]; - for (int i = 0; i < num_threads; ++i) {t[i] = std::thread(call_from_thread,i);} - for (int i = 0; i < num_threads; ++i) {t[i].join();} - for (int i = 0; i < num_threads; ++i) { - reglib::Model * model2 = res[i]; - reglib::FusionResults fr = fr_res[i]; - if(fr.score > 100){ - //fr.guess = fr.guess.inverse(); - fr2merge.push_back(fr); - models2merge.push_back(model2); - printf("%i could be registered\n",i); - } - } - - for(unsigned int i = 0; i < models2merge.size(); i++){ - reglib::Model * model2 = models2merge[i]; - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model2, reg); - mu->occlusion_penalty = occlusion_penalty; - mu->massreg_timeout = massreg_timeout; - mu->viewer = viewer; - reg->visualizationLvl = 0; - - reglib::UpdatedModels ud = mu->fuseData(&(fr2merge[i]), model, model2); - printf("merge %i to %i\n",int(model->id),int(model2->id)); - printf("new_models: %i\n",int(ud.new_models.size())); - printf("updated_models: %i\n",int(ud.updated_models.size())); - printf("deleted_models: %i\n",int(ud.deleted_models.size())); - - delete mu; - delete reg; - - for(unsigned int j = 0; j < ud.new_models.size(); j++){ new_models[ud.new_models[j]->id] = ud.new_models[j];} - for(unsigned int j = 0; j < ud.updated_models.size(); j++){ updated_models[ud.updated_models[j]->id] = ud.updated_models[j];} - - for(unsigned int j = 0; j < ud.deleted_models.size(); j++){ - database->remove(ud.deleted_models[j]); - delete ud.deleted_models[j]; - } - if(ud.deleted_models.size() > 0){changed = true; break;} - } - }else{ -// printf("TIME TO ITERATE THROUGH RESULTS\n"); - for(unsigned int i = 0; i < res.size(); i++){ -// printf("TESTING %i\n",i); - reglib::Model * model2 = res[i]; - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model2, reg); - mu->occlusion_penalty = occlusion_penalty; - mu->massreg_timeout = massreg_timeout; - mu->viewer = viewer; - - mu->show_init_lvl = show_init_lvl;//init show - mu->show_refine_lvl = show_refine_lvl;//refine show - mu->show_scoring = show_scoring;//fuse scoring show - reg->visualizationLvl = show_reg_lvl; - - reglib::FusionResults fr = mu->registerModel(model); - - if(fr.score > 100){ - reglib::UpdatedModels ud = mu->fuseData(&fr, model2, model); -// printf("merge %i to %i\n",int(model->id),int(model2->id)); -// printf("new_models: %i\n",int(ud.new_models.size())); -// printf("updated_models: %i\n",int(ud.updated_models.size())); -// printf("deleted_models: %i\n",int(ud.deleted_models.size())); - - for(unsigned int j = 0; j < ud.new_models.size(); j++){ new_models[ud.new_models[j]->id] = ud.new_models[j];} - for(unsigned int j = 0; j < ud.updated_models.size(); j++){ updated_models[ud.updated_models[j]->id] = ud.updated_models[j];} - - for(unsigned int j = 0; j < ud.deleted_models.size(); j++){ - database->remove(ud.deleted_models[j]); - delete ud.deleted_models[j]; - } - if(ud.deleted_models.size() > 0){ - changed = true; - delete mu; - delete reg; - //printf("DONE TESTING %i\n",i); - break; +bool runSearch(ModelDatabase * database, reglib::Model * model, int number_of_searches = 5){ + quasimodo_msgs::model_to_retrieval_query m2r; + m2r.request.model = quasimodo_brain::getModelMSG(model,true); + if (conversion_client.call(m2r)){ + quasimodo_msgs::query_cloud qc; + qc.request.query = m2r.response.query; + qc.request.query.query_kind = qc.request.query.METAROOM_QUERY; + qc.request.query.number_query = number_of_searches+10; + + if (retrieval_client.call(qc)){ + quasimodo_msgs::retrieval_result result = qc.response.result; + + for(unsigned int i = 0; i < result.retrieved_images.size(); i++){ + for(unsigned int j = 0; j < result.retrieved_images[i].images.size(); j++){ + cv_bridge::CvImagePtr ret_image_ptr; + try {ret_image_ptr = cv_bridge::toCvCopy(result.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + cv_bridge::CvImagePtr ret_mask_ptr; + try {ret_mask_ptr = cv_bridge::toCvCopy(result.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + cv_bridge::CvImagePtr ret_depth_ptr; + try {ret_depth_ptr = cv_bridge::toCvCopy(result.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} + catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + cv::Mat rgbimage = ret_image_ptr->image; + cv::Mat maskimage = ret_mask_ptr->image; + cv::Mat depthimage = ret_depth_ptr->image; + + cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); + cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); + cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", depthimage ); + cv::waitKey( 0 ); } } - delete mu; - delete reg; - - //printf("DONE TESTING %i\n",i); - } - } - - - - for (std::map::iterator it=updated_models.begin(); it!=updated_models.end(); ++it) { - database->remove(it->second); - models_deleted_pub.publish(getModelMSG(it->second)); - } - for (std::map::iterator it=updated_models.begin(); it!=updated_models.end(); ++it) { - if (deleteIfFail) { // deleteIfFail means that this is coming from search results, we delete them if failing to recognize - std_msgs::String msg; - msg.data = "Added search result to DB!"; - chatter_pub.publish(msg); - publishDatabasePCD(); - } - addToDB(database, it->second); - } - for (std::map::iterator it=new_models.begin(); it!=new_models.end(); ++it) { - if (deleteIfFail) { // deleteIfFail means that this is coming from search results, we delete them if failing to recognize - std_msgs::String msg; - msg.data = "Added search result to DB!"; - chatter_pub.publish(msg); - publishDatabasePCD(); - } - addToDB(database, it->second); - } - - //printf("end of addToDB: %i %i",int(add),int(deleteIfFail)); - if(deleteIfFail){ - if(!changed){ - printf("didnt manage to integrate searchresult\n"); - - database->remove(model); - for(unsigned int i = 0; i < model->frames.size(); i++){ - std::string key = model->frames[i]->keyval; - if(framekeys.count(key) != 0){framekeys.erase(key);} - delete model->frames[i]; - delete model->modelmasks[i]; - } - delete model; + // int maxval = 0; + // int maxind = 0; + + // int dilation_size = 0; + // cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, + // cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), + // cv::Point( dilation_size, dilation_size ) ); + // for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ + // //framekeys + + // cv_bridge::CvImagePtr ret_image_ptr; + // try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv_bridge::CvImagePtr ret_mask_ptr; + // try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv_bridge::CvImagePtr ret_depth_ptr; + // try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv::Mat rgbimage = ret_image_ptr->image; + // cv::Mat maskimage = ret_mask_ptr->image; + // cv::Mat depthimage = ret_depth_ptr->image; + + // cv::Mat erosion_dst; + // cv::erode( maskimage, erosion_dst, element ); + + // unsigned short * depthdata = (unsigned short * )depthimage.data; + // unsigned char * rgbdata = (unsigned char * )rgbimage.data; + // unsigned char * maskdata = (unsigned char * )erosion_dst.data; + // int count = 0; + // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ + // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ + // count++; + // } + // } + // if(count > maxval){ + // maxval = count; + // maxind = j; + // } + // } + + // //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ + // if(maxval > 100){//Atleast some pixels has to be in the mask... + // std::string key = sresult.retrieved_image_paths[i].strings[maxind]; + // printf("searchresult key:%s\n",key.c_str()); + // if(framekeys.count(key) == 0){ + + // int j = maxind; + + // cv_bridge::CvImagePtr ret_image_ptr; + // try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv_bridge::CvImagePtr ret_mask_ptr; + // try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv_bridge::CvImagePtr ret_depth_ptr; + // try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv::Mat rgbimage = ret_image_ptr->image; + // cv::Mat maskimage = ret_mask_ptr->image; + // cv::Mat depthimage = ret_depth_ptr->image; + // for (int ii = 0; ii < depthimage.rows; ++ii) { + // for (int jj = 0; jj < depthimage.cols; ++jj) { + // depthimage.at(ii, jj) *= 5; + // } + // } + + + // cv::Mat erosion_dst; + // cv::erode( maskimage, erosion_dst, element ); + + // // cv::Mat overlap = rgbimage.clone(); + // // unsigned short * depthdata = (unsigned short * )depthimage.data; + // // unsigned char * rgbdata = (unsigned char * )rgbimage.data; + // // unsigned char * maskdata = (unsigned char * )maskimage.data; + // // unsigned char * overlapdata = (unsigned char * )overlap.data; + // // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ + // // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ + // // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; + // // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; + // // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; + // // }else{ + // // overlapdata[3*pixel+0] = 0; + // // overlapdata[3*pixel+1] = 0; + // // overlapdata[3*pixel+2] = 0; + // // } + // // } + // // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); + // // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); + // // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); + // // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); + + // Eigen::Affine3d epose = Eigen::Affine3d::Identity(); + // reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); + // frame->keyval = key; + + // // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); + // // frame->show(true); + // //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); + // reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); + // bool res = searchmodel->testFrame(0); + + // reglib::Model * searchmodelHolder = new reglib::Model(); + // searchmodelHolder->submodels.push_back(searchmodel); + // searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + // searchmodelHolder->last_changed = ++current_model_update; + // searchmodelHolder->recomputeModelPoints(); + + // //searchmodelHolder->showHistory(viewer); + + + // // if(cv::waitKey(0) != 'n'){ + // //Todo:: check new model is not flat or L shape + + // printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); + // //addToDB(modeldatabase, searchmodel,false,true); + // addToDB(modeldatabase, searchmodelHolder,false,true); + // show_sorted(); + // } + // } + +// printf("---------------ids in searchresult: %i----------------\n",result.vocabulary_ids.size()); +// for(unsigned int i = 0; i < result.vocabulary_ids.size(); i++){ +// int ind = result.vocabulary_ids[i]; +// printf("found object with ind: %i\n",ind); +// if(vocabulary_ids.count(ind) == 0){ printf("does not exist in db, continue\n"); continue; } +// reglib::Model * search_model = vocabulary_ids[ind]; +// if(search_model != model){printf("adding to search results\n"); ret.push_back(search_model);} +// if(ret.size() == number_of_matches){printf("found enough search results\n"); return ret;} +// } }else{ - printf("integrateing searchresult\n"); + ROS_ERROR("retrieval_client service FAIL in %s :: %i !",__FILE__,__LINE__); } + }else{ + ROS_ERROR("model_to_retrieval_query service FAIL in %s :: %i !",__FILE__,__LINE__); } + return true; } +std::set searchset; void addNewModel(reglib::Model * model){ - //printf("model: %i\n",model->frames.size()); - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model, reg); mu->occlusion_penalty = occlusion_penalty; @@ -690,9 +650,8 @@ void addNewModel(reglib::Model * model){ mu->show_scoring = show_scoring;//fuse scoring show reg->visualizationLvl = show_reg_lvl; - //model->print(); mu->makeInitialSetup(); - //model->print(); + delete mu; delete reg; @@ -705,331 +664,356 @@ void addNewModel(reglib::Model * model){ modeldatabase->add(newmodelHolder); addToDB(modeldatabase, newmodelHolder,false); - for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - publish_history(modeldatabase->models[i]->getHistory()); + show_sorted(); + + bool do_next = true; + while(do_next && run_search){ + printf("running search loop\n"); + do_next = false; + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + reglib::Model * current = modeldatabase->models[i]; + if(searchset.count(current->id)==0){ + searchset.insert(current->id); + printf("new search %i\n",current->id); + + + if(runSearch(modeldatabase, current)){ + do_next = true; + break; + } + }else{ + printf("already searched %i\n",current->id); + } + } } + + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){publish_history(modeldatabase->models[i]->getHistory());} publishDatabasePCD(); dumpDatabase(savepath); -// modeldatabase->add(newmodelHolder); -// addToDB(modeldatabase, newmodelHolder,false); -// for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ -// publish_history(modeldatabase->models[i]->getHistory()); -// } -// /* -// for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ -// publish_history(modeldatabase->models[i]->getHistory()); -// }*/ -// show_sorted(); -// publishDatabasePCD(); -// dumpDatabase(savepath); - - -// modeldatabase->add(model); -// addToDB(modeldatabase, model,false); -// for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ -// publish_history(modeldatabase->models[i]->getHistory()); -// } -// publishDatabasePCD(); -// dumpDatabase(savepath); - - -// for(unsigned int m = 0; run_search && m < modeldatabase->models.size(); m++){ -// printf("looking at: %i\n",int(modeldatabase->models[m]->last_changed)); -// reglib::Model * currentTest = modeldatabase->models[m]; -// if(currentTest->last_changed > current_model_update_before){ -// printf("changed: %i\n",int(m)); - -// double start = getTime(); - -// new_search_result = false; -// models_new_pub.publish(getModelMSG(currentTest)); - -// while(getTime()-start < search_timeout){ -// ros::spinOnce(); -// if(new_search_result){ - -// for(unsigned int i = 0; i < sresult.retrieved_images.size(); i++){ - -// int maxval = 0; -// int maxind = 0; - -// int dilation_size = 0; -// cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, -// cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), -// cv::Point( dilation_size, dilation_size ) ); -// for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ -// //framekeys - -// cv_bridge::CvImagePtr ret_image_ptr; -// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} -// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -// cv_bridge::CvImagePtr ret_mask_ptr; -// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} -// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -// cv_bridge::CvImagePtr ret_depth_ptr; -// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} -// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -// cv::Mat rgbimage = ret_image_ptr->image; -// cv::Mat maskimage = ret_mask_ptr->image; -// cv::Mat depthimage = ret_depth_ptr->image; - -// cv::Mat erosion_dst; -// cv::erode( maskimage, erosion_dst, element ); - -// unsigned short * depthdata = (unsigned short * )depthimage.data; -// unsigned char * rgbdata = (unsigned char * )rgbimage.data; -// unsigned char * maskdata = (unsigned char * )erosion_dst.data; -// int count = 0; -// for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ -// if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ -// count++; -// } -// } -// if(count > maxval){ -// maxval = count; -// maxind = j; -// } -// } - -// //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ -// if(maxval > 100){//Atleast some pixels has to be in the mask... -// std::string key = sresult.retrieved_image_paths[i].strings[maxind]; -// printf("searchresult key:%s\n",key.c_str()); -// if(framekeys.count(key) == 0){ - -// int j = maxind; - -// cv_bridge::CvImagePtr ret_image_ptr; -// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} -// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -// cv_bridge::CvImagePtr ret_mask_ptr; -// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} -// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -// cv_bridge::CvImagePtr ret_depth_ptr; -// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} -// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -// cv::Mat rgbimage = ret_image_ptr->image; -// cv::Mat maskimage = ret_mask_ptr->image; -// cv::Mat depthimage = ret_depth_ptr->image; -// for (int ii = 0; ii < depthimage.rows; ++ii) { -// for (int jj = 0; jj < depthimage.cols; ++jj) { -// depthimage.at(ii, jj) *= 5; -// } -// } - - -// cv::Mat erosion_dst; -// cv::erode( maskimage, erosion_dst, element ); - -// // cv::Mat overlap = rgbimage.clone(); -// // unsigned short * depthdata = (unsigned short * )depthimage.data; -// // unsigned char * rgbdata = (unsigned char * )rgbimage.data; -// // unsigned char * maskdata = (unsigned char * )maskimage.data; -// // unsigned char * overlapdata = (unsigned char * )overlap.data; -// // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ -// // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ -// // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; -// // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; -// // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; -// // }else{ -// // overlapdata[3*pixel+0] = 0; -// // overlapdata[3*pixel+1] = 0; -// // overlapdata[3*pixel+2] = 0; -// // } -// // } -// // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); -// // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); -// // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); -// // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); - -// Eigen::Affine3d epose = Eigen::Affine3d::Identity(); -// reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); -// frame->keyval = key; - -// // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); -// // frame->show(true); -// //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); -// reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); -// bool res = searchmodel->testFrame(0); - -// reglib::Model * searchmodelHolder = new reglib::Model(); -// searchmodelHolder->submodels.push_back(searchmodel); -// searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); -// searchmodelHolder->last_changed = ++current_model_update; -// searchmodelHolder->recomputeModelPoints(); - -// //searchmodelHolder->showHistory(viewer); - - -// // if(cv::waitKey(0) != 'n'){ -// //Todo:: check new model is not flat or L shape - -// printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); -// //addToDB(modeldatabase, searchmodel,false,true); -// addToDB(modeldatabase, searchmodelHolder,false,true); -// show_sorted(); -// } -// } - - -// /* -//// for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ -//// //framekeys - -//// cv_bridge::CvImagePtr ret_image_ptr; -//// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} -//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -//// cv_bridge::CvImagePtr ret_mask_ptr; -//// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} -//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -//// cv_bridge::CvImagePtr ret_depth_ptr; -//// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} -//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -//// cv::Mat rgbimage = ret_image_ptr->image; -//// cv::Mat maskimage = ret_mask_ptr->image; -//// cv::Mat depthimage = ret_depth_ptr->image; - -//// cv::Mat erosion_dst; -//// cv::erode( maskimage, erosion_dst, element ); - -//// unsigned short * depthdata = (unsigned short * )depthimage.data; -//// unsigned char * rgbdata = (unsigned char * )rgbimage.data; -//// unsigned char * maskdata = (unsigned char * )erosion_dst.data; -//// int count = 0; -//// for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ -//// if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ -//// count++; -//// } -//// } -//// if(count > maxval){ -//// maxval = count; -//// maxind = j; -//// } -//// } - -//// //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ -//// if(maxval > 100){//Atleast some pixels has to be in the mask... -//// std::string key = sresult.retrieved_image_paths[i].strings[maxind]; -//// printf("searchresult key:%s\n",key.c_str()); -//// if(framekeys.count(key) == 0){ - -//// int j = maxind; - -//// cv_bridge::CvImagePtr ret_image_ptr; -//// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} -//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -//// cv_bridge::CvImagePtr ret_mask_ptr; -//// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} -//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -//// cv_bridge::CvImagePtr ret_depth_ptr; -//// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} -//// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - -//// cv::Mat rgbimage = ret_image_ptr->image; -//// cv::Mat maskimage = ret_mask_ptr->image; -//// cv::Mat depthimage = ret_depth_ptr->image; -//// for (int ii = 0; ii < depthimage.rows; ++ii) { -//// for (int jj = 0; jj < depthimage.cols; ++jj) { -//// depthimage.at(ii, jj) *= 5; -//// } -//// } - - -//// cv::Mat erosion_dst; -//// cv::erode( maskimage, erosion_dst, element ); - -//// // cv::Mat overlap = rgbimage.clone(); -//// // unsigned short * depthdata = (unsigned short * )depthimage.data; -//// // unsigned char * rgbdata = (unsigned char * )rgbimage.data; -//// // unsigned char * maskdata = (unsigned char * )maskimage.data; -//// // unsigned char * overlapdata = (unsigned char * )overlap.data; -//// // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ -//// // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ -//// // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; -//// // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; -//// // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; -//// // }else{ -//// // overlapdata[3*pixel+0] = 0; -//// // overlapdata[3*pixel+1] = 0; -//// // overlapdata[3*pixel+2] = 0; -//// // } -//// // } -//// // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); -//// // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); -//// // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); -//// // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); - -//// Eigen::Affine3d epose = Eigen::Affine3d::Identity(); -//// reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); -//// frame->keyval = key; -//// frame->show(true); -//// //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); -//// reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); -//// bool res = searchmodel->testFrame(0); - -//// reglib::Model * searchmodelHolder = new reglib::Model(); -//// searchmodelHolder->submodels.push_back(searchmodel); -//// searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); -//// searchmodelHolder->last_changed = ++current_model_update; -//// searchmodelHolder->recomputeModelPoints(); - - -//// // if(cv::waitKey(0) != 'n'){ -//// //Todo:: check new model is not flat or L shape - -//// printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); -//// //addToDB(modeldatabase, searchmodel,false,true); -//// addToDB(modeldatabase, searchmodelHolder,false,true); -//// show_sorted(); -//// // } -//// } - -//// } -// */ -// } - -// break; -// }else{ -// printf("searching... timeout in %3.3f\n", +start +search_timeout - getTime()); -// usleep(100000); -// } -// } -// } -// //exit(0); -// } -// for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ -// for(unsigned int j = 0; j < modeldatabase->models[i]->submodels.size(); j++){ -// if( modeldatabase->models[i]->submodels[j]->frames.front() == frontframe){ -// Eigen::Matrix4d pose = frontframe->pose * modeldatabase->models[i]->submodels_relativeposes[j].inverse(); -// pcl::PointCloud::Ptr cloud = modeldatabase->models[i]->submodels[j]->getPCLcloud(1,true); -// pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); -// pcl::transformPointCloud (*cloud, *transformed_cloud, pose); -// sensor_msgs::PointCloud2 input; -// pcl::toROSMsg (*transformed_cloud,input);//, *transformed_cloud); -// input.header.frame_id = "/map"; -// model_last_pub.publish(input); -// } -// } -// } + // modeldatabase->add(newmodelHolder); + // addToDB(modeldatabase, newmodelHolder,false); + // for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + // publish_history(modeldatabase->models[i]->getHistory()); + // } + // /* + // for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + // publish_history(modeldatabase->models[i]->getHistory()); + // }*/ + // show_sorted(); + // publishDatabasePCD(); + // dumpDatabase(savepath); + + + // modeldatabase->add(model); + // addToDB(modeldatabase, model,false); + // for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + // publish_history(modeldatabase->models[i]->getHistory()); + // } + // publishDatabasePCD(); + // dumpDatabase(savepath); + + + // for(unsigned int m = 0; run_search && m < modeldatabase->models.size(); m++){ + // printf("looking at: %i\n",int(modeldatabase->models[m]->last_changed)); + // reglib::Model * currentTest = modeldatabase->models[m]; + // if(currentTest->last_changed > current_model_update_before){ + // printf("changed: %i\n",int(m)); + + // double start = getTime(); + + // new_search_result = false; + // models_new_pub.publish(getModelMSG(currentTest)); + + // while(getTime()-start < search_timeout){ + // ros::spinOnce(); + // if(new_search_result){ + + // for(unsigned int i = 0; i < sresult.retrieved_images.size(); i++){ + + // int maxval = 0; + // int maxind = 0; + + // int dilation_size = 0; + // cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, + // cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), + // cv::Point( dilation_size, dilation_size ) ); + // for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ + // //framekeys + + // cv_bridge::CvImagePtr ret_image_ptr; + // try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv_bridge::CvImagePtr ret_mask_ptr; + // try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv_bridge::CvImagePtr ret_depth_ptr; + // try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv::Mat rgbimage = ret_image_ptr->image; + // cv::Mat maskimage = ret_mask_ptr->image; + // cv::Mat depthimage = ret_depth_ptr->image; + + // cv::Mat erosion_dst; + // cv::erode( maskimage, erosion_dst, element ); + + // unsigned short * depthdata = (unsigned short * )depthimage.data; + // unsigned char * rgbdata = (unsigned char * )rgbimage.data; + // unsigned char * maskdata = (unsigned char * )erosion_dst.data; + // int count = 0; + // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ + // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ + // count++; + // } + // } + // if(count > maxval){ + // maxval = count; + // maxind = j; + // } + // } + + // //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ + // if(maxval > 100){//Atleast some pixels has to be in the mask... + // std::string key = sresult.retrieved_image_paths[i].strings[maxind]; + // printf("searchresult key:%s\n",key.c_str()); + // if(framekeys.count(key) == 0){ + + // int j = maxind; + + // cv_bridge::CvImagePtr ret_image_ptr; + // try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv_bridge::CvImagePtr ret_mask_ptr; + // try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv_bridge::CvImagePtr ret_depth_ptr; + // try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} + // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + // cv::Mat rgbimage = ret_image_ptr->image; + // cv::Mat maskimage = ret_mask_ptr->image; + // cv::Mat depthimage = ret_depth_ptr->image; + // for (int ii = 0; ii < depthimage.rows; ++ii) { + // for (int jj = 0; jj < depthimage.cols; ++jj) { + // depthimage.at(ii, jj) *= 5; + // } + // } + + + // cv::Mat erosion_dst; + // cv::erode( maskimage, erosion_dst, element ); + + // // cv::Mat overlap = rgbimage.clone(); + // // unsigned short * depthdata = (unsigned short * )depthimage.data; + // // unsigned char * rgbdata = (unsigned char * )rgbimage.data; + // // unsigned char * maskdata = (unsigned char * )maskimage.data; + // // unsigned char * overlapdata = (unsigned char * )overlap.data; + // // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ + // // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ + // // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; + // // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; + // // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; + // // }else{ + // // overlapdata[3*pixel+0] = 0; + // // overlapdata[3*pixel+1] = 0; + // // overlapdata[3*pixel+2] = 0; + // // } + // // } + // // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); + // // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); + // // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); + // // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); + + // Eigen::Affine3d epose = Eigen::Affine3d::Identity(); + // reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); + // frame->keyval = key; + + // // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); + // // frame->show(true); + // //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); + // reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); + // bool res = searchmodel->testFrame(0); + + // reglib::Model * searchmodelHolder = new reglib::Model(); + // searchmodelHolder->submodels.push_back(searchmodel); + // searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + // searchmodelHolder->last_changed = ++current_model_update; + // searchmodelHolder->recomputeModelPoints(); + + // //searchmodelHolder->showHistory(viewer); + + + // // if(cv::waitKey(0) != 'n'){ + // //Todo:: check new model is not flat or L shape + + // printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); + // //addToDB(modeldatabase, searchmodel,false,true); + // addToDB(modeldatabase, searchmodelHolder,false,true); + // show_sorted(); + // } + // } + + + // /* + //// for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ + //// //framekeys + + //// cv_bridge::CvImagePtr ret_image_ptr; + //// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} + //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + //// cv_bridge::CvImagePtr ret_mask_ptr; + //// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} + //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + //// cv_bridge::CvImagePtr ret_depth_ptr; + //// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} + //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + //// cv::Mat rgbimage = ret_image_ptr->image; + //// cv::Mat maskimage = ret_mask_ptr->image; + //// cv::Mat depthimage = ret_depth_ptr->image; + + //// cv::Mat erosion_dst; + //// cv::erode( maskimage, erosion_dst, element ); + + //// unsigned short * depthdata = (unsigned short * )depthimage.data; + //// unsigned char * rgbdata = (unsigned char * )rgbimage.data; + //// unsigned char * maskdata = (unsigned char * )erosion_dst.data; + //// int count = 0; + //// for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ + //// if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ + //// count++; + //// } + //// } + //// if(count > maxval){ + //// maxval = count; + //// maxind = j; + //// } + //// } + + //// //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ + //// if(maxval > 100){//Atleast some pixels has to be in the mask... + //// std::string key = sresult.retrieved_image_paths[i].strings[maxind]; + //// printf("searchresult key:%s\n",key.c_str()); + //// if(framekeys.count(key) == 0){ + + //// int j = maxind; + + //// cv_bridge::CvImagePtr ret_image_ptr; + //// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} + //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + //// cv_bridge::CvImagePtr ret_mask_ptr; + //// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} + //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + //// cv_bridge::CvImagePtr ret_depth_ptr; + //// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} + //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} + + //// cv::Mat rgbimage = ret_image_ptr->image; + //// cv::Mat maskimage = ret_mask_ptr->image; + //// cv::Mat depthimage = ret_depth_ptr->image; + //// for (int ii = 0; ii < depthimage.rows; ++ii) { + //// for (int jj = 0; jj < depthimage.cols; ++jj) { + //// depthimage.at(ii, jj) *= 5; + //// } + //// } + + + //// cv::Mat erosion_dst; + //// cv::erode( maskimage, erosion_dst, element ); + + //// // cv::Mat overlap = rgbimage.clone(); + //// // unsigned short * depthdata = (unsigned short * )depthimage.data; + //// // unsigned char * rgbdata = (unsigned char * )rgbimage.data; + //// // unsigned char * maskdata = (unsigned char * )maskimage.data; + //// // unsigned char * overlapdata = (unsigned char * )overlap.data; + //// // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ + //// // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ + //// // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; + //// // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; + //// // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; + //// // }else{ + //// // overlapdata[3*pixel+0] = 0; + //// // overlapdata[3*pixel+1] = 0; + //// // overlapdata[3*pixel+2] = 0; + //// // } + //// // } + //// // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); + //// // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); + //// // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); + //// // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); + + //// Eigen::Affine3d epose = Eigen::Affine3d::Identity(); + //// reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); + //// frame->keyval = key; + //// frame->show(true); + //// //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); + //// reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); + //// bool res = searchmodel->testFrame(0); + + //// reglib::Model * searchmodelHolder = new reglib::Model(); + //// searchmodelHolder->submodels.push_back(searchmodel); + //// searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + //// searchmodelHolder->last_changed = ++current_model_update; + //// searchmodelHolder->recomputeModelPoints(); + + + //// // if(cv::waitKey(0) != 'n'){ + //// //Todo:: check new model is not flat or L shape + + //// printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); + //// //addToDB(modeldatabase, searchmodel,false,true); + //// addToDB(modeldatabase, searchmodelHolder,false,true); + //// show_sorted(); + //// // } + //// } + + //// } + // */ + // } + + // break; + // }else{ + // printf("searching... timeout in %3.3f\n", +start +search_timeout - getTime()); + // usleep(100000); + // } + // } + // } + // //exit(0); + // } + + // for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + // for(unsigned int j = 0; j < modeldatabase->models[i]->submodels.size(); j++){ + // if( modeldatabase->models[i]->submodels[j]->frames.front() == frontframe){ + // Eigen::Matrix4d pose = frontframe->pose * modeldatabase->models[i]->submodels_relativeposes[j].inverse(); -// newmodel = 0; -// sweepid_counter++; + // pcl::PointCloud::Ptr cloud = modeldatabase->models[i]->submodels[j]->getPCLcloud(1,true); + // pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); + // pcl::transformPointCloud (*cloud, *transformed_cloud, pose); + + + // sensor_msgs::PointCloud2 input; + // pcl::toROSMsg (*transformed_cloud,input);//, *transformed_cloud); + // input.header.frame_id = "/map"; + // model_last_pub.publish(input); + // } + // } + // } + + // newmodel = 0; + // sweepid_counter++; } @@ -1075,7 +1059,7 @@ bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_ res.model_id = newmodel->id; if(isnewmodel != 0){ - reglib::RGBDFrame * frontframe = newmodel->frames.front(); + reglib::RGBDFrame * frontframe = newmodel->frames.front(); int current_model_update_before = current_model_update; newmodel->recomputeModelPoints(); @@ -1086,16 +1070,16 @@ bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_ mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; - mu->show_init_lvl = show_init_lvl;//init show - mu->show_refine_lvl = show_refine_lvl;//refine show - mu->show_scoring = show_scoring;//fuse scoring show - reg->visualizationLvl = show_reg_lvl; + mu->show_init_lvl = show_init_lvl;//init show + mu->show_refine_lvl = show_refine_lvl;//refine show + mu->show_scoring = show_scoring;//fuse scoring show + reg->visualizationLvl = show_reg_lvl; -//newmodel->print(); + //newmodel->print(); mu->makeInitialSetup(); -//newmodel->print(); -delete mu; -delete reg; + //newmodel->print(); + delete mu; + delete reg; reglib::Model * newmodelHolder = new reglib::Model(); newmodelHolder->submodels.push_back(newmodel); @@ -1104,22 +1088,22 @@ delete reg; newmodelHolder->recomputeModelPoints(); modeldatabase->add(newmodelHolder); - addToDB(modeldatabase, newmodelHolder,false); - for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - publish_history(modeldatabase->models[i]->getHistory()); - } - /* - for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - publish_history(modeldatabase->models[i]->getHistory()); - }*/ + addToDB(modeldatabase, newmodelHolder,false); + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + publish_history(modeldatabase->models[i]->getHistory()); + } + /* + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + publish_history(modeldatabase->models[i]->getHistory()); + }*/ show_sorted(); - publishDatabasePCD(); - dumpDatabase(savepath); + publishDatabasePCD(); + dumpDatabase(savepath); -//exit(0); + //exit(0); -/* + /* //SIMPLE TEST reglib::Model * testmodel = new reglib::Model(); for(int i = 0; i < newmodel->frames.size();i++){ @@ -1182,8 +1166,8 @@ for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} printf("\n"); */ -//exit(0); -/* + //exit(0); + /* vector models; vector rps; @@ -1197,7 +1181,7 @@ unsigned int nr_models1 = models.size(); addModelsToVector(models,rps,model2,pose); vector > ocs = computeOcclusionScore(models,rps); */ -/* + /* //newmodel->recomputeModelPoints(); @@ -1323,17 +1307,17 @@ vector > ocs = computeOcclusionScore(models,rps); // overlapdata[3*pixel+2] = 0; // } // } -// cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); -// cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); -// cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); -// cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); + // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); + // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); + // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); + // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); Eigen::Affine3d epose = Eigen::Affine3d::Identity(); reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); frame->keyval = key; -// cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); -// frame->show(true); + // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); + // frame->show(true); //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); bool res = searchmodel->testFrame(0); @@ -1352,13 +1336,13 @@ vector > ocs = computeOcclusionScore(models,rps); printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); //addToDB(modeldatabase, searchmodel,false,true); - addToDB(modeldatabase, searchmodelHolder,false,true); + //addToDB(modeldatabase, searchmodelHolder,false,true); show_sorted(); } } -/* + /* for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ //framekeys @@ -1489,23 +1473,23 @@ vector > ocs = computeOcclusionScore(models,rps); //exit(0); } - for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - for(unsigned int j = 0; j < modeldatabase->models[i]->submodels.size(); j++){ - if( modeldatabase->models[i]->submodels[j]->frames.front() == frontframe){ - Eigen::Matrix4d pose = frontframe->pose * modeldatabase->models[i]->submodels_relativeposes[j].inverse(); + for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ + for(unsigned int j = 0; j < modeldatabase->models[i]->submodels.size(); j++){ + if( modeldatabase->models[i]->submodels[j]->frames.front() == frontframe){ + Eigen::Matrix4d pose = frontframe->pose * modeldatabase->models[i]->submodels_relativeposes[j].inverse(); - pcl::PointCloud::Ptr cloud = modeldatabase->models[i]->submodels[j]->getPCLcloud(1,true); - pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); - pcl::transformPointCloud (*cloud, *transformed_cloud, pose); + pcl::PointCloud::Ptr cloud = modeldatabase->models[i]->submodels[j]->getPCLcloud(1,true); + pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); + pcl::transformPointCloud (*cloud, *transformed_cloud, pose); - sensor_msgs::PointCloud2 input; - pcl::toROSMsg (*transformed_cloud,input);//, *transformed_cloud); - input.header.frame_id = "/map"; - model_last_pub.publish(input); - } - } - } + sensor_msgs::PointCloud2 input; + pcl::toROSMsg (*transformed_cloud,input);//, *transformed_cloud); + input.header.frame_id = "/map"; + model_last_pub.publish(input); + } + } + } newmodel = 0; sweepid_counter++; @@ -1591,10 +1575,18 @@ int main(int argc, char **argv){ ros::Subscriber sub = n.subscribe("/retrieval_result", 1, retrievalCallback); ROS_INFO("Ready to add recieve search results."); - database_pcd_pub = n.advertise("modelserver/databasepcd", 1000); - model_history_pub = n.advertise("modelserver/model_history", 1000); - model_last_pub = n.advertise("modelserver/last", 1000); - model_places_pub = n.advertise("modelserver/model_places", 1000); + database_pcd_pub = n.advertise("modelserver/databasepcd", 1000); + model_history_pub = n.advertise("modelserver/model_history", 1000); + model_last_pub = n.advertise("modelserver/last", 1000); + model_places_pub = n.advertise("modelserver/model_places", 1000); + + std::string retrieval_name = "/quasimodo_retrieval_service"; + std::string conversion_name = "/models/server"; + std::string insert_name = "/insert_model_service"; + retrieval_client = n.serviceClient (retrieval_name); + conversion_client = n.serviceClient (conversion_name); + insert_client = n.serviceClient (insert_name); + std::vector input_model_subs; @@ -1614,11 +1606,11 @@ int main(int argc, char **argv){ else if(std::string(argv[i]).compare("-occlusion_penalty") == 0){printf("occlusion_penalty input state\n");inputstate = 4;} else if(std::string(argv[i]).compare("-massreg_timeout") == 0){printf("massreg_timeout input state\n");inputstate = 5;} else if(std::string(argv[i]).compare("-search") == 0){printf("pointcloud search input state\n");run_search = true; inputstate = 6;} - else if(std::string(argv[i]).compare("-v") == 0){ printf("visualization turned on\n"); visualization = true;} - else if(std::string(argv[i]).compare("-v_init") == 0){ printf("visualization of init turned on\n"); visualization = true; inputstate = 8;} - else if(std::string(argv[i]).compare("-v_refine") == 0 || std::string(argv[i]).compare("-v_ref") == 0){ printf("visualization refinement turned on\n"); visualization = true; inputstate = 9;} - else if(std::string(argv[i]).compare("-v_register") == 0 || std::string(argv[i]).compare("-v_reg") == 0){ printf("visualization registration turned on\n"); visualization = true; inputstate = 10;} - else if(std::string(argv[i]).compare("-v_scoring") == 0 || std::string(argv[i]).compare("-v_score") == 0 || std::string(argv[i]).compare("-v_sco") == 0){ printf("visualization scoring turned on\n"); visualization = true; show_scoring = true;} + else if(std::string(argv[i]).compare("-v") == 0){ printf("visualization turned on\n"); visualization = true;} + else if(std::string(argv[i]).compare("-v_init") == 0){ printf("visualization of init turned on\n"); visualization = true; inputstate = 8;} + else if(std::string(argv[i]).compare("-v_refine") == 0 || std::string(argv[i]).compare("-v_ref") == 0){ printf("visualization refinement turned on\n"); visualization = true; inputstate = 9;} + else if(std::string(argv[i]).compare("-v_register") == 0 || std::string(argv[i]).compare("-v_reg") == 0){ printf("visualization registration turned on\n"); visualization = true; inputstate = 10;} + else if(std::string(argv[i]).compare("-v_scoring") == 0 || std::string(argv[i]).compare("-v_score") == 0 || std::string(argv[i]).compare("-v_sco") == 0){ printf("visualization scoring turned on\n"); visualization = true; show_scoring = true;} else if(std::string(argv[i]).compare("-v_db") == 0){ printf("visualization db turned on\n"); visualization = true; show_db = true;} else if(std::string(argv[i]).compare("-intopic") == 0){ printf("intopic input state\n"); inputstate = 11;} else if(std::string(argv[i]).compare("-mdb") == 0){ printf("intopic input state\n"); inputstate = 12;} @@ -1640,15 +1632,15 @@ int main(int argc, char **argv){ occlusion_penalty = atof(argv[i]); printf("occlusion_penalty set to %f\n",occlusion_penalty); }else if(inputstate == 5){ massreg_timeout = atof(argv[i]); printf("massreg_timeout set to %f\n",massreg_timeout); - }else if(inputstate == 6){ - search_timeout = atof(argv[i]); printf("search_timeout set to %f\n",search_timeout); - if(search_timeout == 0){ - run_search = false; - } - }else if(inputstate == 8){ - show_init_lvl = atoi(argv[i]); - }else if(inputstate == 9){ - show_refine_lvl = atoi(argv[i]); + }else if(inputstate == 6){ + search_timeout = atof(argv[i]); printf("search_timeout set to %f\n",search_timeout); + if(search_timeout == 0){ + run_search = false; + } + }else if(inputstate == 8){ + show_init_lvl = atoi(argv[i]); + }else if(inputstate == 9){ + show_refine_lvl = atoi(argv[i]); }else if(inputstate == 10){ show_reg_lvl = atoi(argv[i]); }else if(inputstate == 11){ diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 7198154e..8746f0b4 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -58,8 +58,8 @@ namespace reglib void save(std::string path = ""); RGBDFrame * clone(); static RGBDFrame * load(Camera * cam, std::string path); + std::vector getReprojections(std::vector & spvec, Eigen::Matrix4d cp); }; - } #endif // reglibRGBDFrame_H diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index 4a4f6ec6..a81cea75 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -55,6 +55,49 @@ using Components = boost::component_index; namespace reglib { +class superpoint{ + public: + Eigen::Vector3f point; + Eigen::Vector3f normal; + Eigen::VectorXf feature; + double point_information; + double feature_information; + int last_update_frame_id; + + superpoint(Eigen::Vector3f p, Eigen::Vector3f n, Eigen::VectorXf f, double pi = 1, double fi = 1, int id = 0){ + point = p; + normal = n; + feature = f; + point_information = pi; + feature_information = fi; + last_update_frame_id = id; + } + + ~superpoint(){} + + void merge(superpoint p, double weight = 1){ + double newpweight = weight*p.point_information + point_information; + double newfweight = weight*p.feature_information + feature_information; +//printf("before: (%3.3f)(%3.3f) + (%3.3f)(%3.3f) -> (%3.3f)(%3.3f)\n", +//feature_information,feature(0),p.feature_information,p.feature(0),(feature_information*feature(0)+p.feature_information*p.feature(0))/(feature_information+p.feature_information)); + + point = weight*p.point_information*p.point + point_information*point; + normal = weight*p.point_information*p.normal + point_information*normal; + //feature = weight*p.feature_information*p.feature + feature_information*feature; + + + normal.normalize(); + + point /= newpweight; + point_information = newpweight; + + //feature /= newfweight; + //feature_information = newfweight; + + last_update_frame_id = std::max(p.last_update_frame_id,last_update_frame_id); + } +}; + class ReprojectionResult{ public: diff --git a/quasimodo/quasimodo_models/include/model/Model.h b/quasimodo/quasimodo_models/include/model/Model.h index d6aa5570..8ce6b011 100644 --- a/quasimodo/quasimodo_models/include/model/Model.h +++ b/quasimodo/quasimodo_models/include/model/Model.h @@ -22,48 +22,48 @@ namespace reglib { using namespace std; using namespace Eigen; -class superpoint{ - public: - Eigen::Vector3f point; - Eigen::Vector3f normal; - Eigen::VectorXf feature; - double point_information; - double feature_information; - int last_update_frame_id; - - superpoint(Eigen::Vector3f p, Eigen::Vector3f n, Eigen::VectorXf f, double pi = 1, double fi = 1, int id = 0){ - point = p; - normal = n; - feature = f; - point_information = pi; - feature_information = fi; - last_update_frame_id = id; - } - - ~superpoint(){} - - void merge(superpoint p, double weight = 1){ - double newpweight = weight*p.point_information + point_information; - double newfweight = weight*p.feature_information + feature_information; -//printf("before: (%3.3f)(%3.3f) + (%3.3f)(%3.3f) -> (%3.3f)(%3.3f)\n", -//feature_information,feature(0),p.feature_information,p.feature(0),(feature_information*feature(0)+p.feature_information*p.feature(0))/(feature_information+p.feature_information)); - - point = weight*p.point_information*p.point + point_information*point; - normal = weight*p.point_information*p.normal + point_information*normal; - //feature = weight*p.feature_information*p.feature + feature_information*feature; - - - normal.normalize(); - - point /= newpweight; - point_information = newpweight; - - //feature /= newfweight; - //feature_information = newfweight; - - last_update_frame_id = std::max(p.last_update_frame_id,last_update_frame_id); - } -}; +//class superpoint{ +// public: +// Eigen::Vector3f point; +// Eigen::Vector3f normal; +// Eigen::VectorXf feature; +// double point_information; +// double feature_information; +// int last_update_frame_id; + +// superpoint(Eigen::Vector3f p, Eigen::Vector3f n, Eigen::VectorXf f, double pi = 1, double fi = 1, int id = 0){ +// point = p; +// normal = n; +// feature = f; +// point_information = pi; +// feature_information = fi; +// last_update_frame_id = id; +// } + +// ~superpoint(){} + +// void merge(superpoint p, double weight = 1){ +// double newpweight = weight*p.point_information + point_information; +// double newfweight = weight*p.feature_information + feature_information; +////printf("before: (%3.3f)(%3.3f) + (%3.3f)(%3.3f) -> (%3.3f)(%3.3f)\n", +////feature_information,feature(0),p.feature_information,p.feature(0),(feature_information*feature(0)+p.feature_information*p.feature(0))/(feature_information+p.feature_information)); + +// point = weight*p.point_information*p.point + point_information*point; +// normal = weight*p.point_information*p.normal + point_information*normal; +// //feature = weight*p.feature_information*p.feature + feature_information*feature; + + +// normal.normalize(); + +// point /= newpweight; +// point_information = newpweight; + +// //feature /= newfweight; +// //feature_information = newfweight; + +// last_update_frame_id = std::max(p.last_update_frame_id,last_update_frame_id); +// } +//}; class Model{ public: diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h index 236e32ed..b843a7e6 100644 --- a/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h +++ b/quasimodo/quasimodo_models/include/registration/MassRegistrationPPR2.h @@ -150,7 +150,7 @@ namespace reglib void setData(std::vector< pcl::PointCloud::Ptr > all_clouds); void rematchKeyPoints(std::vector poses, std::vector prev_poses, bool first); - void rematch(std::vector poses, std::vector prev_poses, bool first); + void rematch(std::vector poses, std::vector prev_poses, bool rematch_surface, bool rematch_edges,bool first); Eigen::MatrixXd getAllResiduals(std::vector poses); Eigen::MatrixXd depthedge_getAllResiduals(std::vector poses); Eigen::MatrixXd getAllKpResiduals(std::vector poses); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index a08db7a6..9747ad33 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -2204,4 +2204,100 @@ RGBDFrame * RGBDFrame::load(Camera * cam, std::string path){ }else{printf("cant open %s\n",(path+"/data.txt").c_str());} } +std::vector RGBDFrame::getReprojections(std::vector & spvec, Eigen::Matrix4d cp){ + std::vector ret; + return ret; +} + +// unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); +// unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); +// float * dst_normalsdata = (float *)(cf->normals.data); + +// float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); +// float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); +// float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); + +// Camera * dst_camera = cf->camera; +// const unsigned int dst_width = dst_camera->width; +// const unsigned int dst_height = dst_camera->height; +// const float dst_idepth = dst_camera->idepth_scale; +// const float dst_cx = dst_camera->cx; +// const float dst_cy = dst_camera->cy; +// const float dst_fx = dst_camera->fx; +// const float dst_fy = dst_camera->fy; +// const float dst_ifx = 1.0/dst_camera->fx; +// const float dst_ify = 1.0/dst_camera->fy; +// const unsigned int dst_width2 = dst_camera->width - 2; +// const unsigned int dst_height2 = dst_camera->height - 2; + + +// std::vector src_inds; +// std::vector dst_inds; +// std::vector residuals; +// std::vector cameraSurfaceAngles; +// std::vector angles; +// src_inds.reserve(nr_data); +// dst_inds.reserve(nr_data); +// residuals.reserve(nr_data); +// cameraSurfaceAngles.reserve(nr_data); +// angles.reserve(nr_data); + +// unsigned long nr_data = spvec.size(); +// for(unsigned long ind = 0; ind < nr_data;ind++){ +// superpoint & sp = spvec[ind]; + +// float src_x = sp.point(0); +// float src_y = sp.point(1); +// float src_z = sp.point(2); + +// float src_nx = sp.normal(0); +// float src_ny = sp.normal(1); +// float src_nz = sp.normal(2); + +// float point_information = sp.point_information; + +// float tx = m00*src_x + m01*src_y + m02*src_z + m03; +// float ty = m10*src_x + m11*src_y + m12*src_z + m13; +// float tz = m20*src_x + m21*src_y + m22*src_z + m23; + +// float itz = 1.0/tz; +// float dst_w = dst_fx*tx*itz + dst_cx; +// float dst_h = dst_fy*ty*itz + dst_cy; + +// if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ +// unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + +// float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); +// float dst_nx = dst_normalsdata[3*dst_ind+0]; +// if(dst_z > 0 && dst_nx != 2){ +// if(dst_detdata[dst_ind] != 0){continue;} +// float dst_ny = dst_normalsdata[3*dst_ind+1]; +// float dst_nz = dst_normalsdata[3*dst_ind+2]; + +// float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; +// float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + +// float tnx = m00*src_nx + m01*src_ny + m02*src_nz; +// float tny = m10*src_nx + m11*src_ny + m12*src_nz; +// float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + +// double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); +// double dst_noise = dst_z * dst_z; +// double point_noise = 1.0/sqrt(point_information); + +// double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); +// d *= compare_mul; + +// double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); +// double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); + +// residuals.push_back(d); +// weights.push_back(angle_dst*angle_dst*angle_dst); +// src_inds.push_back(ind); +// dst_inds.push_back(dst_ind); +// } +// } +// } +//} + } diff --git a/quasimodo/quasimodo_models/src/core/Util.cpp b/quasimodo/quasimodo_models/src/core/Util.cpp index da4c486e..0db59493 100644 --- a/quasimodo/quasimodo_models/src/core/Util.cpp +++ b/quasimodo/quasimodo_models/src/core/Util.cpp @@ -309,96 +309,4 @@ namespace reglib{ if(change_trans < stopvalt && change_rot < stopvalr){return true;} else{return false;} } - - //OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf){ - // unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); - // unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); - // float * dst_normalsdata = (float *)(cf->normals.data); - - // float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); - // float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); - // float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); - - // Camera * dst_camera = cf->camera; - // const unsigned int dst_width = dst_camera->width; - // const unsigned int dst_height = dst_camera->height; - // const float dst_idepth = dst_camera->idepth_scale; - // const float dst_cx = dst_camera->cx; - // const float dst_cy = dst_camera->cy; - // const float dst_fx = dst_camera->fx; - // const float dst_fy = dst_camera->fy; - // const float dst_ifx = 1.0/dst_camera->fx; - // const float dst_ify = 1.0/dst_camera->fy; - // const unsigned int dst_width2 = dst_camera->width - 2; - // const unsigned int dst_height2 = dst_camera->height - 2; - - - // std::vector src_inds; - // std::vector dst_inds; - // std::vector residuals; - // std::vector cameraSurfaceAngles; - // std::vector angles; - // src_inds.reserve(nr_data); - // dst_inds.reserve(nr_data); - // residuals.reserve(nr_data); - // cameraSurfaceAngles.reserve(nr_data); - // angles.reserve(nr_data); - - // unsigned long nr_data = spvec.size(); - // for(unsigned long ind = 0; ind < nr_data;ind++){ - // superpoint & sp = spvec[ind]; - - // float src_x = sp.point(0); - // float src_y = sp.point(1); - // float src_z = sp.point(2); - - // float src_nx = sp.normal(0); - // float src_ny = sp.normal(1); - // float src_nz = sp.normal(2); - - // float point_information = sp.point_information; - - // float tx = m00*src_x + m01*src_y + m02*src_z + m03; - // float ty = m10*src_x + m11*src_y + m12*src_z + m13; - // float tz = m20*src_x + m21*src_y + m22*src_z + m23; - - // float itz = 1.0/tz; - // float dst_w = dst_fx*tx*itz + dst_cx; - // float dst_h = dst_fy*ty*itz + dst_cy; - - // if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - // unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - // float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - // float dst_nx = dst_normalsdata[3*dst_ind+0]; - // if(dst_z > 0 && dst_nx != 2){ - // if(dst_detdata[dst_ind] != 0){continue;} - // float dst_ny = dst_normalsdata[3*dst_ind+1]; - // float dst_nz = dst_normalsdata[3*dst_ind+2]; - - // float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - // float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - // float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - // float tny = m10*src_nx + m11*src_ny + m12*src_nz; - // float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - - // double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - // double dst_noise = dst_z * dst_z; - // double point_noise = 1.0/sqrt(point_information); - - // double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); - // d *= compare_mul; - - // double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); - // double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); - - // residuals.push_back(d); - // weights.push_back(angle_dst*angle_dst*angle_dst); - // src_inds.push_back(ind); - // dst_inds.push_back(dst_ind); - // } - // } - // } - //} } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 8aaa2cbc..adf3cc28 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -89,7 +89,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); float * dst_normalsdata = (float *)(cf->normals.data); - unsigned char * dst_detdata = (unsigned char *)(cf->det_dilate.data); + //unsigned char * dst_detdata = (unsigned char *)(cf->det_dilate.data); float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); @@ -149,7 +149,7 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); float dst_nx = dst_normalsdata[3*dst_ind+0]; if(dst_z > 0 && dst_nx != 2){ - if(dst_detdata[dst_ind] != 0){continue;} + //if(dst_detdata[dst_ind] != 0){continue;} float dst_ny = dst_normalsdata[3*dst_ind+1]; float dst_nz = dst_normalsdata[3*dst_ind+2]; diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index f1316ee3..1e6fb371 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -570,7 +570,8 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ return good/(good+bad+0.001); } -void MassRegistrationPPR2::rematch(std::vector poses, std::vector prev_poses, bool first){ +void MassRegistrationPPR2::rematch(std::vector poses, std::vector prev_poses, bool rematch_surface, bool rematch_edges, bool first){ + if(!rematch_surface && !rematch_edges){return;} double new_good_rematches = 0; double new_total_rematches = 0; unsigned long nr_frames = poses.size(); @@ -618,10 +619,10 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect double ratiosum = 0; - if(use_depthedge && depthedge_nr_arraypoints[i] > 10 && depthedge_nr_arraypoints[j] > 10){ + if(rematch_edges && depthedge_nr_arraypoints[i] > 10 && depthedge_nr_arraypoints[j] > 10){ ratiosum += matchframes(depthedge_func,rp, depthedge_nr_arraypoints[i], depthedge_arraypoints[i], depthedge_matchids[i][j], depthedge_matchdists[i][j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); } - if(use_surface && nr_arraypoints[i] > 10 && nr_arraypoints[j] > 10){ + if(rematch_surface && nr_arraypoints[i] > 10 && nr_arraypoints[j] > 10){ ratiosum += matchframes(func,rp, nr_arraypoints[i], arraypoints[i], matchids[i][j], matchdists[i][j],trees3d[j], new_good_rematches,new_total_rematches); } @@ -3170,14 +3171,14 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses1 = poses; double rematch_time_start = getTime(); - rematch(poses,poses0,first); - //rematchKeyPoints(poses,poses0,first); + //rematch(poses,poses0,use_surface,use_depthedge,first); + rematch(poses,poses0,use_surface,false,first); first = false; poses0 = poses; rematch_time += getTime()-rematch_time_start; for(long lala = 0; lala < 10; lala++){ - if(visualizationLvl > 0){ + if(visualizationLvl > 0){ printf("funcupdate: %i rematching: %i lala: %i\n",funcupdate,rematching,lala); printf("total_time: %5.5f\n",getTime()-total_time_start); printf("rematch_time: %5.5f\n",rematch_time); @@ -3196,6 +3197,10 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses2b = poses; double residuals_time_start = getTime(); @@ -3278,11 +3283,11 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectornoiseval > 10.0*func->regularization && ratio > 0.75){break;} } -// printf("total_time: %5.5f\n",getTime()-total_time_start); -// printf("rematch_time: %5.5f\n",rematch_time); -// printf("compM residuals_time:%5.5f\n",residuals_time); -// printf("computeModel: %5.5f\n",computeModel_time); -// printf("opt_time: %5.5f\n",opt_time); + printf("total_time: %5.5f\n",getTime()-total_time_start); + printf("rematch_time: %5.5f\n",rematch_time); + printf("compM residuals_time:%5.5f\n",residuals_time); + printf("computeModel: %5.5f\n",computeModel_time); + printf("opt_time: %5.5f\n",opt_time); // printf("setup_matches_time: %5.5f\n",setup_matches_time); // printf("setup_equation_time: %5.5f\n",setup_equation_time); From 53c3420859ff8dfd49aba3e5eec3c69337ef527d Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 12 Sep 2016 22:11:01 +0200 Subject: [PATCH 100/255] working on surface merging --- quasimodo/quasimodo_brain/src/modelserver.cpp | 12 +- quasimodo/quasimodo_models/CMakeLists.txt | 12 +- .../include/core/OcclusionScore.h | 31 +++ .../quasimodo_models/include/core/RGBDFrame.h | 6 +- .../include/core/ReprojectionResult.h | 28 ++ .../quasimodo_models/include/core/Util.h | 90 ++---- .../include/core/superpoint.h | 25 ++ .../quasimodo_models/include/model/Model.h | 47 +--- .../src/core/OcclusionScore.cpp | 17 ++ .../quasimodo_models/src/core/RGBDFrame.cpp | 244 ++++++++++------- .../src/core/ReprojectionResult.cpp | 20 ++ quasimodo/quasimodo_models/src/core/Util.cpp | 34 +++ .../quasimodo_models/src/core/superpoint.cpp | 29 ++ .../quasimodo_models/src/model/Model.cpp | 256 ++++++++++-------- .../src/modelupdater/ModelUpdater.cpp | 1 - 15 files changed, 523 insertions(+), 329 deletions(-) create mode 100644 quasimodo/quasimodo_models/include/core/OcclusionScore.h create mode 100644 quasimodo/quasimodo_models/include/core/ReprojectionResult.h create mode 100644 quasimodo/quasimodo_models/include/core/superpoint.h create mode 100644 quasimodo/quasimodo_models/src/core/OcclusionScore.cpp create mode 100644 quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp create mode 100644 quasimodo/quasimodo_models/src/core/superpoint.cpp diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index e2762ffc..7bab9cd9 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -61,6 +61,7 @@ int show_refine_lvl = 0;//refine show int show_reg_lvl = 0;//registration show bool show_scoring = false;//fuse scoring show bool show_search = false; +bool show_modelbuild = false; std::map cameras; @@ -660,7 +661,11 @@ void addNewModel(reglib::Model * model){ newmodelHolder->submodels.push_back(model); newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); newmodelHolder->last_changed = ++current_model_update; - newmodelHolder->recomputeModelPoints(); + if(show_modelbuild){ + newmodelHolder->recomputeModelPoints(Eigen::Matrix4d::Identity(),viewer); + }else{ + newmodelHolder->recomputeModelPoints(); + } modeldatabase->add(newmodelHolder); addToDB(modeldatabase, newmodelHolder,false); @@ -1615,6 +1620,7 @@ int main(int argc, char **argv){ else if(std::string(argv[i]).compare("-intopic") == 0){ printf("intopic input state\n"); inputstate = 11;} else if(std::string(argv[i]).compare("-mdb") == 0){ printf("intopic input state\n"); inputstate = 12;} else if(std::string(argv[i]).compare("-show_search") == 0){ printf("show_search\n"); show_search = true;} + else if(std::string(argv[i]).compare("-show_modelbuild") == 0){ printf("show_modelbuild\n"); visualization = true; show_modelbuild = true;} else if(inputstate == 1){ reglib::Camera * cam = reglib::Camera::load(std::string(argv[i])); delete cameras[0]; @@ -1677,12 +1683,12 @@ int main(int argc, char **argv){ ros::Duration(1.0).sleep(); chatter_pub = n.advertise("modelserver", 1000); - sleep(1); +// sleep(1); std_msgs::String msg; msg.data = "starting"; chatter_pub.publish(msg); ros::spinOnce(); - sleep(1); +// sleep(1); ros::spin(); return 0; diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index e17ffd61..114dad03 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -55,8 +55,18 @@ INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) add_library(quasimodo_weightlib src/weightfunctions/DistanceWeightFunction2.cpp src/weightfunctions/DistanceWeightFunction2PPR.cpp src/weightfunctions/DistanceWeightFunction2PPR2.cpp) target_link_libraries(quasimodo_weightlib ceres glog ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +add_library(quasimodo_superpoint src/core/superpoint.cpp) +target_link_libraries(quasimodo_superpoint ${catkin_LIBRARIES}) + +add_library(quasimodo_ReprojectionResult src/core/ReprojectionResult.cpp) +target_link_libraries(quasimodo_ReprojectionResult quasimodo_superpoint ${catkin_LIBRARIES}) + +add_library(quasimodo_OcclusionScore src/core/OcclusionScore.cpp) +target_link_libraries(quasimodo_OcclusionScore ${catkin_LIBRARIES}) + + add_library(quasimodo_core src/core/Util.cpp src/core/Camera.cpp src/core/RGBDFrame.cpp) -target_link_libraries(quasimodo_core quasimodo_weightlib ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries(quasimodo_core quasimodo_OcclusionScore quasimodo_ReprojectionResult quasimodo_superpoint quasimodo_weightlib ${CERES_LIBRARIES} ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) add_library(quasimodo_reglib src/registration/Registration.cpp src/registration/RegistrationRandom.cpp src/registration/RegistrationRefinement.cpp) target_link_libraries(quasimodo_reglib quasimodo_weightlib ceres glog ${catkin_LIBRARIES}) diff --git a/quasimodo/quasimodo_models/include/core/OcclusionScore.h b/quasimodo/quasimodo_models/include/core/OcclusionScore.h new file mode 100644 index 00000000..7109406d --- /dev/null +++ b/quasimodo/quasimodo_models/include/core/OcclusionScore.h @@ -0,0 +1,31 @@ +#ifndef reglibOcclusionScore_H +#define reglibOcclusionScore_H + +#include +#include +#include +#include +#include +#include + +#include "OcclusionScore.h" + +namespace reglib +{ + +class OcclusionScore{ + public: + double score; + double occlusions; + + OcclusionScore(); + OcclusionScore( double score_ ,double occlusions_); + ~OcclusionScore(); + + void add(OcclusionScore oc); + void print(); +}; + +} + +#endif diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 8746f0b4..3c9cff2c 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -20,6 +20,7 @@ #include #include +#include "superpoint.h" #include "Util.h" #include "Camera.h" #include "../weightfunctions/DistanceWeightFunction2.h" @@ -53,12 +54,15 @@ namespace reglib void show(bool stop = false); pcl::PointCloud::Ptr getPCLcloud(); + pcl::PointCloud::Ptr getSmallPCLcloud(); void savePCD(std::string path = "cloud.pcd", Eigen::Matrix4d pose = Eigen::Matrix4d::Identity()); void save(std::string path = ""); RGBDFrame * clone(); static RGBDFrame * load(Camera * cam, std::string path); - std::vector getReprojections(std::vector & spvec, Eigen::Matrix4d cp); + std::vector getReprojections(std::vector & spvec, Eigen::Matrix4d cp, bool * maskvec, bool useDet = true); + + std::vector getSuperPoints(Eigen::Matrix4d cp = Eigen::Matrix4d::Identity()); }; } diff --git a/quasimodo/quasimodo_models/include/core/ReprojectionResult.h b/quasimodo/quasimodo_models/include/core/ReprojectionResult.h new file mode 100644 index 00000000..1507ff3a --- /dev/null +++ b/quasimodo/quasimodo_models/include/core/ReprojectionResult.h @@ -0,0 +1,28 @@ +#ifndef reglibReprojectionResult_H +#define reglibReprojectionResult_H + +#include + + +namespace reglib +{ +class ReprojectionResult{ + public: + unsigned long src_ind; + unsigned long dst_ind; + double angle; + double residualZ; + double residualR; + double residualG; + double residualB; + + double noiseZ; + double noiseRGB; + + ReprojectionResult(); + ReprojectionResult(unsigned long si, unsigned long di, double rz, double a, double rr, double rg, double rb, double nz, double nrgb); + ~ReprojectionResult(); +}; +} + +#endif diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index a81cea75..0f4f88be 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -34,6 +34,13 @@ #include #include +#include +#include + +#include "superpoint.h" +#include "ReprojectionResult.h" +#include "OcclusionScore.h" + using ceres::NumericDiffCostFunction; using ceres::SizedCostFunction; using ceres::CENTRAL; @@ -55,81 +62,7 @@ using Components = boost::component_index; namespace reglib { -class superpoint{ - public: - Eigen::Vector3f point; - Eigen::Vector3f normal; - Eigen::VectorXf feature; - double point_information; - double feature_information; - int last_update_frame_id; - - superpoint(Eigen::Vector3f p, Eigen::Vector3f n, Eigen::VectorXf f, double pi = 1, double fi = 1, int id = 0){ - point = p; - normal = n; - feature = f; - point_information = pi; - feature_information = fi; - last_update_frame_id = id; - } - - ~superpoint(){} - - void merge(superpoint p, double weight = 1){ - double newpweight = weight*p.point_information + point_information; - double newfweight = weight*p.feature_information + feature_information; -//printf("before: (%3.3f)(%3.3f) + (%3.3f)(%3.3f) -> (%3.3f)(%3.3f)\n", -//feature_information,feature(0),p.feature_information,p.feature(0),(feature_information*feature(0)+p.feature_information*p.feature(0))/(feature_information+p.feature_information)); - - point = weight*p.point_information*p.point + point_information*point; - normal = weight*p.point_information*p.normal + point_information*normal; - //feature = weight*p.feature_information*p.feature + feature_information*feature; - - - normal.normalize(); - - point /= newpweight; - point_information = newpweight; - - //feature /= newfweight; - //feature_information = newfweight; - - last_update_frame_id = std::max(p.last_update_frame_id,last_update_frame_id); - } -}; - -class ReprojectionResult{ - public: - unsigned long src_ind; - unsigned long dst_ind; - double residual; - double angle; - - ReprojectionResult(unsigned long si, unsigned long di, double r, double a){ - src_ind = si; - dst_ind = di; - residual = r; - angle = a; - } -}; - -class OcclusionScore{ - public: - double score; - double occlusions; - - OcclusionScore(){score = 0;occlusions = 0;} - OcclusionScore( double score_ ,double occlusions_){score = score_;occlusions = occlusions_;} - ~OcclusionScore(){} - - void add(OcclusionScore oc){ - score += oc.score; - occlusions += oc.occlusions; - } - - void print(){printf("score: %5.5f occlusions: %5.5f\n",score,occlusions);} -}; float graph_cut(std::vector& graphs_out,std::vector>& second_graphinds, Graph& graph_in, std::vector graph_inds); @@ -137,6 +70,15 @@ float recursive_split(std::vector * graphs_out,std::vector partition_graph(std::vector< std::vector< float > > & scores); +inline double getNoise(double depth){return depth*depth;} +inline double getInformation(double depth){ + double n = getNoise(depth); + return 1.0/(n*n); +} + +pcl::PointCloud::Ptr getPointCloudFromVector(std::vector & spvec, int colortype = 0); + + /** * @brief Returns homogenous 4x4 transformation matrix for given rotation (quaternion) and translation components * @param q rotation represented as quaternion diff --git a/quasimodo/quasimodo_models/include/core/superpoint.h b/quasimodo/quasimodo_models/include/core/superpoint.h new file mode 100644 index 00000000..ec0fec85 --- /dev/null +++ b/quasimodo/quasimodo_models/include/core/superpoint.h @@ -0,0 +1,25 @@ +#ifndef reglibsuperpoint_H +#define reglibsuperpoint_H + +#include + + +namespace reglib +{ +class superpoint{ +public: + Eigen::Vector3f point; + Eigen::Vector3f normal; + Eigen::VectorXf feature; + double point_information; + double normal_information; + double feature_information; + int last_update_frame_id; + + superpoint(Eigen::Vector3f p = Eigen::Vector3f(0,0,0), Eigen::Vector3f n = Eigen::Vector3f(0,0,0), Eigen::VectorXf f = Eigen::VectorXf(3), double pi = 1, double fi = 1, int id = 0); + ~superpoint(); + void merge(superpoint p, double weight = 1); +}; +} + +#endif diff --git a/quasimodo/quasimodo_models/include/model/Model.h b/quasimodo/quasimodo_models/include/model/Model.h index 8ce6b011..07430e96 100644 --- a/quasimodo/quasimodo_models/include/model/Model.h +++ b/quasimodo/quasimodo_models/include/model/Model.h @@ -22,48 +22,7 @@ namespace reglib { using namespace std; using namespace Eigen; -//class superpoint{ -// public: -// Eigen::Vector3f point; -// Eigen::Vector3f normal; -// Eigen::VectorXf feature; -// double point_information; -// double feature_information; -// int last_update_frame_id; -// superpoint(Eigen::Vector3f p, Eigen::Vector3f n, Eigen::VectorXf f, double pi = 1, double fi = 1, int id = 0){ -// point = p; -// normal = n; -// feature = f; -// point_information = pi; -// feature_information = fi; -// last_update_frame_id = id; -// } - -// ~superpoint(){} - -// void merge(superpoint p, double weight = 1){ -// double newpweight = weight*p.point_information + point_information; -// double newfweight = weight*p.feature_information + feature_information; -////printf("before: (%3.3f)(%3.3f) + (%3.3f)(%3.3f) -> (%3.3f)(%3.3f)\n", -////feature_information,feature(0),p.feature_information,p.feature(0),(feature_information*feature(0)+p.feature_information*p.feature(0))/(feature_information+p.feature_information)); - -// point = weight*p.point_information*p.point + point_information*point; -// normal = weight*p.point_information*p.normal + point_information*normal; -// //feature = weight*p.feature_information*p.feature + feature_information*feature; - - -// normal.normalize(); - -// point /= newpweight; -// point_information = newpweight; - -// //feature /= newfweight; -// //feature_information = newfweight; - -// last_update_frame_id = std::max(p.last_update_frame_id,last_update_frame_id); -// } -//}; class Model{ public: @@ -104,9 +63,9 @@ using namespace Eigen; std::vector::Ptr> getHistory(); - void addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask); - void addAllSuperPoints(vector & spvec, Eigen::Matrix4d pose); - void recomputeModelPoints(Eigen::Matrix4d pose = Eigen::Matrix4d::Identity()); + void addSuperPoints( vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, boost::shared_ptr viewer = 0); + void addAllSuperPoints( vector & spvec, Eigen::Matrix4d pose, boost::shared_ptr viewer = 0); + void recomputeModelPoints(Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(), boost::shared_ptr viewer = 0); void addPointsToModel(RGBDFrame * frame, ModelMask * modelmask, Eigen::Matrix4d p); //void addFrameToModel(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d p); diff --git a/quasimodo/quasimodo_models/src/core/OcclusionScore.cpp b/quasimodo/quasimodo_models/src/core/OcclusionScore.cpp new file mode 100644 index 00000000..f01fd22d --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/OcclusionScore.cpp @@ -0,0 +1,17 @@ +#include "core/OcclusionScore.h" + +namespace reglib +{ + +OcclusionScore::OcclusionScore(){score = 0;occlusions = 0;} +OcclusionScore::OcclusionScore( double score_ ,double occlusions_){score = score_;occlusions = occlusions_;} +OcclusionScore::~OcclusionScore(){} + +void OcclusionScore::add(OcclusionScore oc){ + score += oc.score; + occlusions += oc.occlusions; +} + +void OcclusionScore::print(){printf("score: %5.5f occlusions: %5.5f\n",score,occlusions);} + +} diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 9747ad33..0ef3cf3b 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -2032,6 +2032,39 @@ void RGBDFrame::show(bool stop){ if(stop){ cv::waitKey(0);}else{ cv::waitKey(30);} } +pcl::PointCloud::Ptr RGBDFrame::getSmallPCLcloud(){ + unsigned char * rgbdata = (unsigned char *)rgb.data; + unsigned short * depthdata = (unsigned short *)depth.data; + + const unsigned int width = camera->width; const unsigned int height = camera->height; + const double idepth = camera->idepth_scale; + const double cx = camera->cx; const double cy = camera->cy; + const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->width = 0; + cloud->height = 1; + cloud->points.reserve(width*height); + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + double z = idepth*double(depthdata[ind]); + if(z > 0){ + pcl::PointXYZRGB p; + p.b = rgbdata[3*ind+0]; + p.g = rgbdata[3*ind+1]; + p.r = rgbdata[3*ind+2]; + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + cloud->points.push_back(p); + } + } + } + cloud->width = cloud->points.size(); + return cloud; +} + pcl::PointCloud::Ptr RGBDFrame::getPCLcloud(){ unsigned char * rgbdata = (unsigned char *)rgb.data; unsigned short * depthdata = (unsigned short *)depth.data; @@ -2204,100 +2237,129 @@ RGBDFrame * RGBDFrame::load(Camera * cam, std::string path){ }else{printf("cant open %s\n",(path+"/data.txt").c_str());} } -std::vector RGBDFrame::getReprojections(std::vector & spvec, Eigen::Matrix4d cp){ +std::vector RGBDFrame::getReprojections(std::vector & spvec, Eigen::Matrix4d cp, bool * maskvec, bool useDet){ std::vector ret; + + unsigned char * dst_detdata = (unsigned char *)(det_dilate.data); + unsigned char * dst_rgbdata = (unsigned char *)(rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(depth.data); + float * dst_normalsdata = (float *)(normals.data); + + float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); + float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); + float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); + + Camera * dst_camera = camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + unsigned long nr_data = spvec.size(); + for(unsigned long src_ind = 0; src_ind < nr_data;src_ind++){ + superpoint & sp = spvec[src_ind]; + + float src_x = sp.point(0); + float src_y = sp.point(1); + float src_z = sp.point(2); + + float src_nx = sp.normal(0); + float src_ny = sp.normal(1); + float src_nz = sp.normal(2); + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + float itz = 1.0/tz; + float dst_w = dst_fx*tx*itz + dst_cx; + float dst_h = dst_fy*ty*itz + dst_cy; + + if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ + unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); + if(maskvec != 0 && maskvec[dst_ind] == 0){continue;} + + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + if(useDet && dst_detdata[dst_ind] != 0){continue;} + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + float tnx = m00*src_nx + m01*src_ny + m02*src_nz; + float tny = m10*src_nx + m11*src_ny + m12*src_nz; + float tnz = m20*src_nx + m21*src_ny + m22*src_nz; + + double residualZ = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); + double residualR = dst_rgbdata[3*dst_ind + 2] - sp.feature(0); + double residualG = dst_rgbdata[3*dst_ind + 1] - sp.feature(1); + double residualB = dst_rgbdata[3*dst_ind + 0] - sp.feature(2); + double angle = tnx*dst_nx + tny*dst_ny + tnz*dst_nz; + ret.push_back(ReprojectionResult (src_ind, dst_ind, angle, residualZ, residualR, residualG, residualB, getNoise(dst_z), 1.0)); + } + } + } + return ret; +} + +std::vector RGBDFrame::getSuperPoints(Eigen::Matrix4d cp){ + unsigned char * rgbdata = (unsigned char *)(rgb.data); + unsigned short * depthdata = (unsigned short *)(depth.data); + float * normalsdata = (float *)(normals.data); + + float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); + float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); + float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); + + const unsigned int width = camera->width; + const unsigned int height = camera->height; + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + std::vector ret; + ret.resize(width*height); + for(unsigned long h = 0; h < height;h++){ + for(unsigned long w = 0; w < width;w++){ + unsigned int ind = h * width + w; + //superpoint & p = ret[ind]; + Eigen::Vector3f rgb (rgbdata[3*ind+0],rgbdata[3*ind+1],rgbdata[3*ind+2]); + float z = idepth*float(depthdata[ind]); + float nx = normalsdata[3*ind+0]; + if(z > 0 && nx != 2){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + float ny = normalsdata[3*ind+1]; + float nz = normalsdata[3*ind+2]; + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + float tnx = m00*nx + m01*ny + m02*nz; + float tny = m10*nx + m11*ny + m12*nz; + float tnz = m20*nx + m21*ny + m22*nz; + + ret[ind] = superpoint(Vector3f(tx,ty,tz),Vector3f(tnx,tny,tnz),rgb, getInformation(z), 1, 0); + }else{ + ret[ind] = superpoint(Eigen::Vector3f(0,0,0),Eigen::Vector3f(0,0,0),rgb, 0, 1, 0); + } + } + } return ret; } -// unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); -// unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); -// float * dst_normalsdata = (float *)(cf->normals.data); - -// float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); -// float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); -// float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); - -// Camera * dst_camera = cf->camera; -// const unsigned int dst_width = dst_camera->width; -// const unsigned int dst_height = dst_camera->height; -// const float dst_idepth = dst_camera->idepth_scale; -// const float dst_cx = dst_camera->cx; -// const float dst_cy = dst_camera->cy; -// const float dst_fx = dst_camera->fx; -// const float dst_fy = dst_camera->fy; -// const float dst_ifx = 1.0/dst_camera->fx; -// const float dst_ify = 1.0/dst_camera->fy; -// const unsigned int dst_width2 = dst_camera->width - 2; -// const unsigned int dst_height2 = dst_camera->height - 2; - - -// std::vector src_inds; -// std::vector dst_inds; -// std::vector residuals; -// std::vector cameraSurfaceAngles; -// std::vector angles; -// src_inds.reserve(nr_data); -// dst_inds.reserve(nr_data); -// residuals.reserve(nr_data); -// cameraSurfaceAngles.reserve(nr_data); -// angles.reserve(nr_data); - -// unsigned long nr_data = spvec.size(); -// for(unsigned long ind = 0; ind < nr_data;ind++){ -// superpoint & sp = spvec[ind]; - -// float src_x = sp.point(0); -// float src_y = sp.point(1); -// float src_z = sp.point(2); - -// float src_nx = sp.normal(0); -// float src_ny = sp.normal(1); -// float src_nz = sp.normal(2); - -// float point_information = sp.point_information; - -// float tx = m00*src_x + m01*src_y + m02*src_z + m03; -// float ty = m10*src_x + m11*src_y + m12*src_z + m13; -// float tz = m20*src_x + m21*src_y + m22*src_z + m23; - -// float itz = 1.0/tz; -// float dst_w = dst_fx*tx*itz + dst_cx; -// float dst_h = dst_fy*ty*itz + dst_cy; - -// if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ -// unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - -// float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); -// float dst_nx = dst_normalsdata[3*dst_ind+0]; -// if(dst_z > 0 && dst_nx != 2){ -// if(dst_detdata[dst_ind] != 0){continue;} -// float dst_ny = dst_normalsdata[3*dst_ind+1]; -// float dst_nz = dst_normalsdata[3*dst_ind+2]; - -// float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; -// float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - -// float tnx = m00*src_nx + m01*src_ny + m02*src_nz; -// float tny = m10*src_nx + m11*src_ny + m12*src_nz; -// float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - -// double d = mysign(dst_z-tz)*fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); -// double dst_noise = dst_z * dst_z; -// double point_noise = 1.0/sqrt(point_information); - -// double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); -// d *= compare_mul; - -// double dist_dst = sqrt(dst_x*dst_x+dst_y*dst_y+dst_z*dst_z); -// double angle_dst = fabs((dst_x*dst_nx+dst_y*dst_ny+dst_z*dst_nz)/dist_dst); - -// residuals.push_back(d); -// weights.push_back(angle_dst*angle_dst*angle_dst); -// src_inds.push_back(ind); -// dst_inds.push_back(dst_ind); -// } -// } -// } -//} } diff --git a/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp b/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp new file mode 100644 index 00000000..f471705a --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp @@ -0,0 +1,20 @@ +#include "../../include/core/ReprojectionResult.h" + +namespace reglib +{ + +ReprojectionResult::ReprojectionResult(){} +ReprojectionResult::ReprojectionResult(unsigned long si, unsigned long di, double a, double rz, double rr, double rg, double rb, double nz, double nrgb){ + src_ind = si; + dst_ind = di; + angle = a; + + residualZ = rz; + residualR = rr; + residualG = rg; + residualB = rb; +} +ReprojectionResult::~ReprojectionResult(){} + +} + diff --git a/quasimodo/quasimodo_models/src/core/Util.cpp b/quasimodo/quasimodo_models/src/core/Util.cpp index 0db59493..64981fcb 100644 --- a/quasimodo/quasimodo_models/src/core/Util.cpp +++ b/quasimodo/quasimodo_models/src/core/Util.cpp @@ -1,6 +1,40 @@ #include "core/Util.h" namespace reglib{ + pcl::PointCloud::Ptr getPointCloudFromVector(std::vector & spvec, int colortype){ + unsigned long nr_points = spvec.size(); + pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); + cloud_ptr->width = nr_points; + cloud_ptr->height = 1; + cloud_ptr->points.resize(nr_points); + for(unsigned long ind = 0; ind < nr_points; ind++){ + superpoint & p = spvec[ind]; + pcl::PointXYZRGBNormal & p0 = cloud_ptr->points[ind]; + p0.x = p.point(0); + p0.y = p.point(1); + p0.z = p.point(2); + if(colortype == 0){ + p0.r = p.feature(0); + p0.g = p.feature(1); + p0.b = p.feature(2); + } + if(colortype == 1){ + p0.r = 255.0*fabs(p.normal(0)); + p0.g = 255.0*fabs(p.normal(1)); + p0.b = 255.0*fabs(p.normal(2)); + } + if(colortype == 2){ + p0.r = 255.0*std::min(1.0,1.0/sqrt(p.point_information)); + p0.g = 255.0*std::min(1.0,std::max(0.0,1.0-sqrt(p.point_information))); + p0.b = 0; + } + p0.normal_x = p.normal(0); + p0.normal_y = p.normal(1); + p0.normal_z = p.normal(2); + } + return cloud_ptr; + } + double mysign(double v){ if(v < 0){return -1;} return 1; diff --git a/quasimodo/quasimodo_models/src/core/superpoint.cpp b/quasimodo/quasimodo_models/src/core/superpoint.cpp new file mode 100644 index 00000000..d0b5c98e --- /dev/null +++ b/quasimodo/quasimodo_models/src/core/superpoint.cpp @@ -0,0 +1,29 @@ +#include "../../include/core/superpoint.h" + +namespace reglib +{ + +superpoint::superpoint(Eigen::Vector3f p, Eigen::Vector3f n, Eigen::VectorXf f, double pi, double fi, int id){ + point = p; + normal = n; + feature = f; + point_information = pi; + feature_information = fi; + last_update_frame_id = id; +} + +superpoint::~superpoint(){} + +void superpoint::merge(superpoint p, double weight){ + double newpweight = weight*p.point_information + point_information; + double newfweight = weight*p.feature_information + feature_information; + point = weight*p.point_information*p.point + point_information*point; + normal = weight*p.point_information*p.normal + point_information*normal; + normal.normalize(); + point /= newpweight; + point_information = newpweight; + feature_information = newfweight; + last_update_frame_id = std::max(p.last_update_frame_id,last_update_frame_id); +} +} + diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index c024c7bd..cb37b4ed 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -44,8 +44,130 @@ void Model::getData(std::vector & po, std::vector & } } -void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask){ +void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* frame, ModelMask* modelmask, boost::shared_ptr viewer){ + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + unsigned long nr_pixels = width*height; + + std::vector sumw; + sumw.resize(nr_pixels); + for(unsigned long ind = 0; ind < nr_pixels;ind++){sumw[ind] = 0;} + + std::vector isfused; + isfused.resize(nr_pixels); + for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} + bool * maskvec = modelmask->maskvec; + std::vector rr_vec = frame->getReprojections(spvec,p.inverse(),modelmask->maskvec,false); + std::vector framesp = frame->getSuperPoints(p); + + + unsigned long nr_rr = rr_vec.size(); + printf("nr_rr: %i\n",nr_rr); + if(nr_rr > 10){ + std::vector residualsZ; + double stdval = 0; + for(unsigned long ind = 0; ind < nr_rr;ind++){ + ReprojectionResult & rr = rr_vec[ind]; + superpoint & src_p = spvec[rr.src_ind]; + superpoint & dst_p = framesp[rr.dst_ind]; + double src_variance = 1.0/src_p.point_information; + double dst_variance = 1.0/dst_p.point_information; + double total_variance = src_variance+dst_variance; + double total_stdiv = sqrt(total_variance); + double d = rr.residualZ; + residualsZ.push_back(d/total_stdiv); + stdval += residualsZ.back()*residualsZ.back(); + } + stdval = sqrt(stdval/double(nr_rr)); + + DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + func->zeromean = true; + func->maxp = 0.99; + func->startreg = 0.001; + func->debugg_print = true; + + func->maxd = 0.1; + func->startmaxd = func->maxd; + + func->histogram_size = 100; + func->starthistogram_size = func->histogram_size; +// func->blurval = 0.005; + func->stdval2 = stdval; + func->maxnoise = stdval; + func->reset(); + ((DistanceWeightFunction2*)func)->computeModel(residualsZ); + Eigen::VectorXd probs = ((DistanceWeightFunction2*)func)->getProbs(residualsZ); + delete func; + + //superpoint & src_p = spvec[rr.src_ind]; + //superpoint & dst_p = framesp[rr.dst_ind]; + + for(unsigned long ind = 0; ind < nr_rr;ind++){ + printf("%5.5f -> %3.3f \n",double(ind+1)/double(nr_rr),probs(ind)); + if(probs(ind) > 0.5){ + sumw[rr_vec[ind].dst_ind] += probs(ind); + } + } + + + for(unsigned long ind = 0; ind < nr_rr;ind++){ + if(probs(ind) > 0.5){ + printf("%5.5f -> %3.3f \n",double(ind+1)/double(nr_rr),probs(ind)); + ReprojectionResult & rr = rr_vec[ind]; + superpoint & src_p = spvec[rr.src_ind]; + superpoint & dst_p = framesp[rr.dst_ind]; + float weight = probs(ind)/sumw[rr.dst_ind]; + src_p.merge(dst_p,weight); + } + } + } + + for(unsigned long ind = 0; ind < nr_pixels;ind++){ + if(maskvec[ind] != 0 && sumw[ind] < 0.5){ + spvec.push_back(framesp[ind]); + } + } + + + + if(viewer != 0){ + pcl::PointCloud::Ptr framecloud = getPointCloudFromVector(framesp,1); + viewer->removeAllPointClouds(); + viewer->addPointCloud (framecloud, pcl::visualization::PointCloudColorHandlerRGBField(framecloud), "framecloud"); + viewer->spin(); + + viewer->removeAllPointClouds(); + pcl::PointCloud::Ptr cloud = getPointCloudFromVector(spvec,1); + for(unsigned long ind = 0; ind < cloud->points.size();ind++){ + cloud->points[ind].z -= 1; + } + + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "model"); + viewer->spin(); + + + viewer->removeAllPointClouds(); + viewer->removeAllShapes(); + for(unsigned long ind = 0; ind < nr_rr;ind++){ + if(rand()%100 == 0){ + char buf [1024]; + sprintf(buf,"line%i",ind); + ReprojectionResult & rr = rr_vec[ind]; + viewer->addLine ( cloud->points[rr.src_ind], framecloud->points[rr.dst_ind],255,0,255,buf); + } + } + + viewer->addPointCloud (framecloud, pcl::visualization::PointCloudColorHandlerRGBField(framecloud), "framecloud"); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "model"); + + viewer->spin(); + viewer->removeAllPointClouds(); + viewer->removeAllShapes(); + } + +/* unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); //float * rgbdata = frame->rgbdata; unsigned short * depthdata = (unsigned short *)(frame->depth.data); @@ -63,9 +185,7 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr float im10 = ip(1,0); float im11 = ip(1,1); float im12 = ip(1,2); float im13 = ip(1,3); float im20 = ip(2,0); float im21 = ip(2,1); float im22 = ip(2,2); float im23 = ip(2,3); - Camera * camera = frame->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; + const unsigned int dst_width2 = camera->width - 2; const unsigned int dst_height2 = camera->height - 2; @@ -84,8 +204,6 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr //bool * isfused = new bool[width*height]; for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} - //printf("wh:%i %i\n",width,height); - for(unsigned int ind = 0; ind < spvec.size();ind++){ superpoint & sp = spvec[ind]; @@ -209,17 +327,27 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr } } } + +// if(viewer != 0){ +//// pcl::PointCloud::Ptr framecloud = getPointCloudFromVector(framesp); +//// viewer->removeAllPointClouds(); +//// viewer->addPointCloud (framecloud, pcl::visualization::PointCloudColorHandlerRGBField(framecloud), "framecloud"); +//// viewer->spin(); + +// viewer->removeAllPointClouds(); +// pcl::PointCloud::Ptr cloud = getPCLcloud(1, false); +// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "model"); +// viewer->spin(); +// } //delete[] isfused; + */ } void Model::showHistory(boost::shared_ptr viewer){ //viewer->setBackgroundColor(0.01*(rand()%100),0.01*(rand()%100),0.01*(rand()%100)); for(unsigned int i = 0; i < submodels.size(); i++){ - - pcl::PointCloud::Ptr room_cloud (new pcl::PointCloud); - for(unsigned int j = 0; j < submodels[i]->frames.size(); j++){ bool * maskvec = submodels[i]->modelmasks[j]->maskvec; unsigned char * rgbdata = (unsigned char *)(submodels[i]->frames[j]->rgb.data); @@ -375,19 +503,19 @@ std::vector::Ptr> ret; return ret; } -void Model::addAllSuperPoints(std::vector & spvec, Eigen::Matrix4d pose){ +void Model::addAllSuperPoints(std::vector & spvec, Eigen::Matrix4d pose, boost::shared_ptr viewer){ for(unsigned int i = 0; i < frames.size(); i++){ - addSuperPoints(spvec, pose*relativeposes[i], frames[i], modelmasks[i]); + addSuperPoints(spvec, pose*relativeposes[i], frames[i], modelmasks[i], viewer); } for(unsigned int i = 0; i < submodels.size(); i++){ - submodels[i]->addAllSuperPoints(spvec, pose*submodels_relativeposes[i]); + submodels[i]->addAllSuperPoints(spvec, pose*submodels_relativeposes[i], viewer); } } -void Model::recomputeModelPoints(Eigen::Matrix4d pose){ +void Model::recomputeModelPoints(Eigen::Matrix4d pose, boost::shared_ptr viewer){ points.clear(); - addAllSuperPoints(points,pose); + addAllSuperPoints(points,pose,viewer); } void Model::addPointsToModel(RGBDFrame * frame, ModelMask * modelmask, Eigen::Matrix4d p){ @@ -676,106 +804,6 @@ pcl::PointCloud::Ptr Model::getPCLcloud(int step, bool color){ cloud_ptr->points.push_back(p); } return cloud_ptr; - // if(color){ - // for(unsigned int i = 0; i < points.size(); i+=step){ - // superpoint & sp = points[i]; - // pcl::PointXYZRGB p; - // p.x = sp.point(0); - // p.y = sp.point(1); - // p.z = sp.point(2); - // if(color){ - // p.b = 0; - // p.g = 255; - // p.r = 0; - // }else{ - // p.r = sp.feature(0); - // p.g = sp.feature(1); - // p.b = sp.feature(2); - // } - // cloud_ptr->points.push_back(p); - // } - // }else{ -/* - std::map mymapR; - std::map mymapG; - std::map mymapB; - for(unsigned int f = 0; f < frames.size(); f++){ - //unsigned char * maskdata = (unsigned char *)(masks[f].data); - bool * maskvec = modelmasks[f]->maskvec; - unsigned char * rgbdata = (unsigned char *)(frames[f]->rgb.data); - unsigned short * depthdata = (unsigned short *)(frames[f]->depth.data); - float * normalsdata = (float *)(frames[f]->normals.data); - - Eigen::Matrix4d p = relativeposes[f]; - - int sweepid = modelmasks[f]->sweepid; - - int pr,pg,pb; - if(sweepid == -1){ - pr = rand()%256; - pg = rand()%256; - pb = rand()%256; - }else{ - if (mymapR.count(sweepid)==0){ - mymapR[sweepid] = rand()%256; - mymapG[sweepid] = rand()%256; - mymapB[sweepid] = rand()%256; - } - pr = mymapR[sweepid]; - pg = mymapG[sweepid]; - pb = mymapB[sweepid]; - } - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - Camera * camera = frames[f]->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - - - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - //if(maskdata[ind] == 255){ - if(maskvec[ind]){ - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (w - cx) * z * ifx; - float y = (h - cy) * z * ify; - - float px = m00*x + m01*y + m02*z + m03; - float py = m10*x + m11*y + m12*z + m13; - float pz = m20*x + m21*y + m22*z + m23; - - pcl::PointXYZRGB p; - p.x = px; - p.y = py; - p.z = pz; - if(color){ - p.b = rgbdata[3*ind+0]; - p.g = rgbdata[3*ind+1]; - p.r = rgbdata[3*ind+2]; - }else{#include - p.b = pb; - p.g = pg; - p.r = pr; - } - cloud_ptr->points.push_back(p); - } - } - } - } - } - //} - return cloud_ptr; -*/ } void Model::save(std::string path){ diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index adf3cc28..5fa95499 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -89,7 +89,6 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M unsigned char * dst_rgbdata = (unsigned char *)(cf->rgb.data); unsigned short * dst_depthdata = (unsigned short *)(cf->depth.data); float * dst_normalsdata = (float *)(cf->normals.data); - //unsigned char * dst_detdata = (unsigned char *)(cf->det_dilate.data); float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); From e034e096d45f2c431794c6694f24b7037b34d4ae Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 13 Sep 2016 13:41:17 +0200 Subject: [PATCH 101/255] Workaround for the mongodb message size problem, might turn this into a package eventually --- .../msg/fused_world_state_object.msg | 1 + .../scripts/insert_object_server.py | 18 +++++++- .../src/quasimodo_retrieval_node.cpp | 41 +++++++++++++++++++ 3 files changed, 59 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg index 02c33b4b..4ca4638a 100644 --- a/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg +++ b/quasimodo/quasimodo_msgs/msg/fused_world_state_object.msg @@ -9,3 +9,4 @@ sensor_msgs/Image[] masks geometry_msgs/Pose[] transforms string inserted_at string removed_at +string associated_mongodb_fields_map diff --git a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py index 74936178..85f5a5ab 100755 --- a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py +++ b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py @@ -15,13 +15,14 @@ from observation_registration_services.srv import ProcessRegisteredViews, ProcessRegisteredViewsRequest, ProcessRegisteredViewsResponse from soma_io.state import World, Object from datetime import datetime, timedelta -from quasimodo_msgs.msg import fused_world_state_object +from quasimodo_msgs.msg import fused_world_state_object, image_array from quasimodo_msgs.srv import transform_cloud, transform_cloudRequest, transform_cloudResponse from quasimodo_msgs.srv import index_cloud, index_cloudRequest, index_cloudResponse from quasimodo_msgs.srv import mask_pointclouds, mask_pointcloudsRequest, mask_pointcloudsResponse from quasimodo_msgs.srv import insert_model, insert_modelRequest, insert_modelResponse from geometry_msgs.msg import Pose import time +import json def insert_model_cb(sreq): msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') @@ -73,6 +74,21 @@ def insert_model_cb(sreq): new_obj.inserted_at = now.strftime("%Y-%m-%d %H:%M:%S") new_obj.transforms = local_poses + new_obj_depths = image_array() + new_obj_images = image_array() + new_obj_masks = image_array() + for rgbd_frame, mask in zip(sreq.model.frames, sreq.model.masks): + new_obj_depths.images.append(rgbd_frame.depth) + new_obj_images.images.append(rgbd_frame.rgb) + new_obj_masks.images.append(mask) + + new_obj_associated_mongodb_fields = {} + new_obj_associated_mongodb_fields["depths"] = msg_store.insert(new_obj_depths) + new_obj_associated_mongodb_fields["images"] = msg_store.insert(new_obj_images) + new_obj_associated_mongodb_fields["masks"] = msg_store.insert(new_obj_masks) + + new_obj.associated_mongodb_fields_map = json.dumps(new_obj_associated_mongodb_fields, ensure_ascii=True) + # TODO: mongodb does not support including all of this because of the size # for rgbd_frame, mask in zip(sreq.model.frames, sreq.model.masks): # new_obj.depths.append(rgbd_frame.depth) diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index 28fba39d..f3414055 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -48,6 +48,44 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (HistT, (float[N], histogram, histogram) ) +namespace cereal +{ +//! Loading for std::map for text based archives +template +typename std::enable_if::value, void>::type +load(Archive& ar, std::map& map) +{ + map.clear(); + + auto hint = map.begin(); + while (true) { + const auto namePtr = ar.getNodeName(); + if(!namePtr) { + break; + } + std::string key = namePtr; + std::string value; ar( value ); + hint = map.emplace_hint( hint, std::move( key ), std::move( value ) ); + } +} +} // namespace cereal + +template +Msg1 get_associated_mongodb_field(mongodb_store::MessageStoreProxy& message_store, const Msg2& message, const string& field) +{ + stringstream associated_mongodb_fields_map; + associated_mongodb_fields_map << "{\"value0\": " << message.associated_mongodb_fields_map << "}"; + map associated_mongodb_fields; + { + cereal::JSONInputArchive archive_i(associated_mongodb_fields_map); + archive_i(associated_mongodb_fields); + } + string mongodb_id = associated_mongodb_fields.at(field); + pair, mongo::BSONObj> query = message_store.queryID(mongodb_id); + return *query.first; +} + + template class retrieval_node { public: @@ -264,6 +302,9 @@ class retrieval_node { mongodb_store::MessageStoreProxy message_store_quasimodo(n, "quasimodo", "world_state"); pair, mongo::BSONObj> query = message_store_quasimodo.queryID(mongodb_id); + query.first->depths = get_associated_mongodb_field(message_store_quasimodo, *query.first, "depths").images; + query.first->images = get_associated_mongodb_field(message_store_quasimodo, *query.first, "images").images; + query.first->masks = get_associated_mongodb_field(message_store_quasimodo, *query.first, "masks").images; for (int i = 0; i < query.first->images.size(); ++i) { cv_bridge::CvImagePtr cv_mask_ptr; From 845d202b21143c68a47f454f1d95a572e823658d Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 13 Sep 2016 13:45:44 +0200 Subject: [PATCH 102/255] updated model reconstruction --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 118 ++------ .../metaroom_additional_view_processing.cpp | 63 +++-- .../include/modelupdater/ModelUpdater.h | 33 --- .../quasimodo_models/src/model/Model.cpp | 264 ++---------------- .../src/modelupdater/ModelUpdater.cpp | 137 +++++---- .../src/registration/MassRegistrationPPR2.cpp | 51 +++- 6 files changed, 202 insertions(+), 464 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 5ac6739a..77107133 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -156,58 +156,22 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ cout << m2 << endl << endl; std::vector current_room_frames; - for (size_t i=0; i 8){continue;} + for (size_t i=0; ifx = 532.158936; -// cam->fy = 533.819214; -// cam->cx = 310.514310; -// cam->cy = 236.842039; - image_geometry::PinholeCameraModel aCameraModel = roomData.vIntermediateRoomCloudCamParamsCorrected[i]; - //rc->initializeCamera(aCameraModel.fx(), aCameraModel.fy(), aCameraModel.cx(), aCameraModel.cy(), aCameraModel.fullResolution().width, aCameraModel.fullResolution().height); - + reglib::Camera * cam = new reglib::Camera(); cam->fx = aCameraModel.fx(); cam->fy = aCameraModel.fy(); cam->cx = aCameraModel.cx(); cam->cy = aCameraModel.cy(); cam->print(); - //aCameraModel.fullResolution().width, aCameraModel.fullResolution().height); - - - // cout<<"Intermediate cloud size "<points.size()<recomputeModelPoints(); - printf("nr points: %i\n",sweepmodel->points.size()); + sweepmodel->recomputeModelPoints(); + printf("sweep nr points: %i\n",sweepmodel->points.size()); return sweepmodel; } @@ -243,28 +207,33 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec massregmod->nomask = true; massregmod->stopval = 0.0001; +printf("%s::%i\n",__FILE__,__LINE__); reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); mu->occlusion_penalty = 15; mu->massreg_timeout = 60*4; mu->viewer = viewer; +printf("%s::%i\n",__FILE__,__LINE__); if(models.size() > 0 && bg->frames.size() > 0){ std::vector cpmod; - - bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); - +printf("%s::%i\n",__FILE__,__LINE__); + //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + bg->recomputeModelPoints(); +printf("%s::%i\n",__FILE__,__LINE__); cpmod.push_back(Eigen::Matrix4d::Identity());//,models.front()->relativeposes.front().inverse() * bg->relativeposes.front()); massregmod->addModel(bg); printf("bg->points = %i\n",bg->points.size()); - +printf("%s::%i\n",__FILE__,__LINE__); for(int j = 0; j < models.size(); j++){ - models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + printf("%s::%i\n",__FILE__,__LINE__); + //models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + models[j]->recomputeModelPoints(); cpmod.push_back(bg->frames.front()->pose.inverse() * models[j]->frames.front()->pose);//bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); massregmod->addModel(models[j]); } - +printf("%s::%i\n",__FILE__,__LINE__); reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); for(int j = 0; j < models.size(); j++){ @@ -278,19 +247,22 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; } } + printf("%s::%i\n",__FILE__,__LINE__); }else if(models.size() > 1){ std::vector cpmod; - +printf("%s::%i\n",__FILE__,__LINE__); Eigen::Matrix4d first = Eigen::Matrix4d::Identity(); //Eigen::Matrix4d first = mod_po_vec.front().front(); for(int j = 0; j < models.size(); j++){ - models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + //models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); + models[j]->recomputeModelPoints(); cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); massregmod->addModel(models[j]); } - +printf("%s::%i\n",__FILE__,__LINE__); reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); for(int j = 0; j < models.size(); j++){ + printf("%s::%i\n",__FILE__,__LINE__); Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ @@ -301,8 +273,9 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; } } + printf("%s::%i\n",__FILE__,__LINE__); } - +printf("%s::%i\n",__FILE__,__LINE__); delete massregmod; std::vector bgcp; @@ -313,7 +286,7 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec bgcf.push_back(bg->frames[k]); bgmask.push_back(bg->modelmasks[k]->getMask()); } - +printf("%s::%i\n",__FILE__,__LINE__); for(int j = 0; j < models.size(); j++){ reglib::Model * model = models[j]; @@ -327,54 +300,13 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} masks.push_back(mask); } - +printf("%s::%i\n",__FILE__,__LINE__); std::vector movemask; std::vector dynmask; mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions external.push_back(movemask); internal.push_back(masks); dynamic.push_back(dynmask); - -// for(unsigned int i = 0; i < model->frames.size(); i++){ -// cv::imshow( "rgb", model->frames[i]->rgb ); -// cv::imshow( "movemask", movemask[i] ); -// cv::imshow( "dynmask", dynmask[i] ); -// cv::waitKey(0); -// } - -// std::vector internal_masks = mu->computeDynamicObject(bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,model->relativeposes,model->frames,masks,debugg);//Determine self occlusions -// std::vector external_masks = mu->computeDynamicObject(model->relativeposes,model->frames,masks,bgcp,bgcf,bgmask,model->relativeposes,model->frames,masks,debugg);//Determine external occlusions -// std::vector dynamic_masks; -// for(unsigned int i = 0; i < model->frames.size(); i++){ -// reglib::RGBDFrame * frame = model->frames[i]; -// reglib::Camera * cam = frame->camera; -// cv::Mat mask; -// mask.create(cam->height,cam->width,CV_8UC1); -// unsigned char * maskdata = (unsigned char *)(mask.data); -// for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} - -// unsigned char * internalmaskdata = (unsigned char *)(internal_masks[i].data); -// unsigned char * externalmaskdata = (unsigned char *)(external_masks[i].data); -// for(unsigned int k = 0; k < cam->height * cam->width;k++){ -// if(externalmaskdata[k] == 0 && internalmaskdata[k] != 0 ){ -// maskdata[k] = 255; -// }else{ -// maskdata[k] = 0; -// } -// } - -// dynamic_masks.push_back(mask); - -// // cv::imshow( "rgb", frame->rgb ); -// // cv::imshow( "internal_masks", internal_masks[i] ); -// // cv::imshow( "externalmask", external_masks[i] ); -// // cv::imshow( "dynamic_mask", dynamic_masks[i] ); -// // cv::waitKey(0); -// } - -// internal.push_back(internal_masks); -// external.push_back(external_masks); -// dynamic.push_back(dynamic_masks); } delete reg; diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index d33695f8..ee505e81 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -495,43 +495,48 @@ reglib::Model * processAV(std::string path){ mu->massreg_timeout = 60*4; mu->viewer = viewer; - sweep->points = mu->getSuperPoints(sweep->relativeposes,sweep->frames,sweep->modelmasks,1,false); - - //Not needed if metaroom well calibrated - reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.15); - bgmassreg->timeout = 20; - bgmassreg->viewer = viewer; - bgmassreg->use_surface = true; - bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = 0; - bgmassreg->maskstep = 5; - bgmassreg->nomaskstep = 5; - bgmassreg->nomask = true; - bgmassreg->stopval = 0.0005; - bgmassreg->addModel(sweep); - bgmassreg->setData(frames,masks); - reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); - delete bgmassreg; - - for(unsigned int i = 0; i < frames.size(); i++){ - frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; - } + //sweep->points = mu->getSuperPoints(sweep->relativeposes,sweep->frames,sweep->modelmasks,1,false); + sweep->recomputeModelPoints(); reglib::Model * fullmodel = new reglib::Model(); fullmodel->frames = sweep->frames; fullmodel->relativeposes = sweep->relativeposes; fullmodel->modelmasks = sweep->modelmasks; - for(unsigned int i = 0; i < frames.size(); i++){ - fullmodel->frames.push_back(frames[i]); - fullmodel->modelmasks.push_back(masks[i]); - fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); - } + if(frames.size() > 0){ + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.15); + bgmassreg->timeout = 20; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = false; + bgmassreg->visualizationLvl = 0; + bgmassreg->maskstep = 5; + bgmassreg->nomaskstep = 5; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->addModel(sweep); + bgmassreg->setData(frames,masks); + + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); + delete bgmassreg; + + for(unsigned int i = 0; i < frames.size(); i++){ + frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; + } + + for(unsigned int i = 0; i < frames.size(); i++){ + fullmodel->frames.push_back(frames[i]); + fullmodel->modelmasks.push_back(masks[i]); + fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); + } + } delete reg; delete mu; delete sweep; + fullmodel->recomputeModelPoints(); + return fullmodel; } @@ -628,7 +633,8 @@ void processMetaroom(std::string path){ std::vector fr; std::vector mm; fullmodel->getData(po, fr, mm); - fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); + //fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); + fullmodel->recomputeModelPoints(); SimpleXMLParser parser; SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); @@ -651,7 +657,8 @@ void processMetaroom(std::string path){ printf("prev: %s\n",prev.c_str()); reglib::Model * bg = quasimodo_brain::load_metaroom_model(prev); - bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + bg->recomputeModelPoints(); std::vector< reglib::Model * > models; models.push_back(fullmodel); diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 27d0c6e3..0a952270 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -42,22 +42,6 @@ namespace reglib using namespace Eigen; using namespace cv; - -// class ReprojectionResult{ -// public: -// unsigned long src_ind; -// unsigned long dst_ind; -// double residual; -// double angle; - -// ReprojectionResult(unsigned long si, unsigned long di, double r, double a){ -// src_ind = si; -// dst_ind = di; -// residual = r; -// angle = a; -// } -// }; - class UpdatedModels{ public: std::vector< Model * > new_models; @@ -68,23 +52,6 @@ namespace reglib UpdatedModels(){} }; -// class OcclusionScore{ -// public: -// double score; -// double occlusions; - -// OcclusionScore(){score = 0;occlusions = 0;} -// OcclusionScore( double score_ ,double occlusions_){score = score_;occlusions = occlusions_;} -// ~OcclusionScore(){} - -// void add(OcclusionScore oc){ -// score += oc.score; -// occlusions += oc.occlusions; -// } - -// void print(){printf("score: %5.5f occlusions: %5.5f\n",score,occlusions);} -// }; - class ModelUpdater{ public: double occlusion_penalty; diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index cb37b4ed..6a0fadfd 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -48,7 +48,7 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr Camera * camera = frame->camera; const unsigned int width = camera->width; const unsigned int height = camera->height; - unsigned long nr_pixels = width*height; + unsigned long nr_pixels = width*height; std::vector sumw; sumw.resize(nr_pixels); @@ -58,13 +58,11 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr isfused.resize(nr_pixels); for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} - bool * maskvec = modelmask->maskvec; - std::vector rr_vec = frame->getReprojections(spvec,p.inverse(),modelmask->maskvec,false); - std::vector framesp = frame->getSuperPoints(p); - + bool * maskvec = modelmask->maskvec; + std::vector rr_vec = frame->getReprojections(spvec,p.inverse(),modelmask->maskvec,false); + std::vector framesp = frame->getSuperPoints(p); unsigned long nr_rr = rr_vec.size(); - printf("nr_rr: %i\n",nr_rr); if(nr_rr > 10){ std::vector residualsZ; double stdval = 0; @@ -86,261 +84,64 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr func->zeromean = true; func->maxp = 0.99; func->startreg = 0.001; - func->debugg_print = true; - + func->debugg_print = false; func->maxd = 0.1; func->startmaxd = func->maxd; - func->histogram_size = 100; func->starthistogram_size = func->histogram_size; -// func->blurval = 0.005; func->stdval2 = stdval; func->maxnoise = stdval; func->reset(); ((DistanceWeightFunction2*)func)->computeModel(residualsZ); - Eigen::VectorXd probs = ((DistanceWeightFunction2*)func)->getProbs(residualsZ); - delete func; - - //superpoint & src_p = spvec[rr.src_ind]; - //superpoint & dst_p = framesp[rr.dst_ind]; for(unsigned long ind = 0; ind < nr_rr;ind++){ - printf("%5.5f -> %3.3f \n",double(ind+1)/double(nr_rr),probs(ind)); - if(probs(ind) > 0.5){ - sumw[rr_vec[ind].dst_ind] += probs(ind); - } + double p = func->getProb(residualsZ[ind]); + if(p > 0.5){sumw[rr_vec[ind].dst_ind] += p;} } - for(unsigned long ind = 0; ind < nr_rr;ind++){ - if(probs(ind) > 0.5){ - printf("%5.5f -> %3.3f \n",double(ind+1)/double(nr_rr),probs(ind)); + double p = func->getProb(residualsZ[ind]); + if(p > 0.5){ ReprojectionResult & rr = rr_vec[ind]; superpoint & src_p = spvec[rr.src_ind]; superpoint & dst_p = framesp[rr.dst_ind]; - float weight = probs(ind)/sumw[rr.dst_ind]; + float weight = p/sumw[rr.dst_ind]; src_p.merge(dst_p,weight); } } + delete func; } for(unsigned long ind = 0; ind < nr_pixels;ind++){ if(maskvec[ind] != 0 && sumw[ind] < 0.5){ - spvec.push_back(framesp[ind]); + superpoint & sp = framesp[ind]; + if(sp.point_information > 0){ + spvec.push_back(sp); + } } } - - if(viewer != 0){ - pcl::PointCloud::Ptr framecloud = getPointCloudFromVector(framesp,1); - viewer->removeAllPointClouds(); - viewer->addPointCloud (framecloud, pcl::visualization::PointCloudColorHandlerRGBField(framecloud), "framecloud"); - viewer->spin(); - - viewer->removeAllPointClouds(); pcl::PointCloud::Ptr cloud = getPointCloudFromVector(spvec,1); - for(unsigned long ind = 0; ind < cloud->points.size();ind++){ - cloud->points[ind].z -= 1; - } - - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "model"); - viewer->spin(); - - - viewer->removeAllPointClouds(); - viewer->removeAllShapes(); - for(unsigned long ind = 0; ind < nr_rr;ind++){ - if(rand()%100 == 0){ - char buf [1024]; - sprintf(buf,"line%i",ind); - ReprojectionResult & rr = rr_vec[ind]; - viewer->addLine ( cloud->points[rr.src_ind], framecloud->points[rr.dst_ind],255,0,255,buf); - } - } - - viewer->addPointCloud (framecloud, pcl::visualization::PointCloudColorHandlerRGBField(framecloud), "framecloud"); viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "model"); - viewer->spin(); viewer->removeAllPointClouds(); - viewer->removeAllShapes(); - } - -/* - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - //float * rgbdata = frame->rgbdata; - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); - - unsigned int frameid = frame->id; - - Matrix4d ip = p.inverse(); - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - float im00 = ip(0,0); float im01 = ip(0,1); float im02 = ip(0,2); float im03 = ip(0,3); - float im10 = ip(1,0); float im11 = ip(1,1); float im12 = ip(1,2); float im13 = ip(1,3); - float im20 = ip(2,0); float im21 = ip(2,1); float im22 = ip(2,2); float im23 = ip(2,3); - - - - const unsigned int dst_width2 = camera->width - 2; - const unsigned int dst_height2 = camera->height - 2; - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float fx = camera->fx; - const float fy = camera->fy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - - std::vector isfused; - isfused.resize(width*height); - - //bool * isfused = new bool[width*height]; - for(unsigned int i = 0; i < width*height; i++){isfused[i] = false;} - - for(unsigned int ind = 0; ind < spvec.size();ind++){ - superpoint & sp = spvec[ind]; - - float src_x = sp.point(0); - float src_y = sp.point(1); - float src_z = sp.point(2); - - float src_nx = sp.normal(0); - float src_ny = sp.normal(1); - float src_nz = sp.normal(2); - - float src_r = sp.feature(0); - float src_g = sp.feature(1); - float src_b = sp.feature(2); - - double point_information = sp.point_information; - double feature_information = sp.feature_information; - - float tx = im00*src_x + im01*src_y + im02*src_z + im03; - float ty = im10*src_x + im11*src_y + im12*src_z + im13; - float tz = im20*src_x + im21*src_y + im22*src_z + im23; - - float itz = 1.0/tz; - float dst_w = fx*tx*itz + cx; - float dst_h = fy*ty*itz + cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * width + unsigned(dst_w+0.5); - - float dst_z = idepth*float(depthdata[dst_ind]); - float dst_nx = normalsdata[3*dst_ind+0]; - if(!isfused[dst_ind] && dst_z > 0 && dst_nx != 2){ - float dst_ny = normalsdata[3*dst_ind+1]; - float dst_nz = normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - cx) * dst_z * ifx; - float dst_y = (float(dst_h) - cy) * dst_z * ify; - - double dst_noise = dst_z * dst_z; - - double point_noise = 1.0/sqrt(point_information); - - float tnx = im00*src_nx + im01*src_ny + im02*src_nz; - float tny = im10*src_nx + im11*src_ny + im12*src_nz; - float tnz = im20*src_nx + im21*src_ny + im22*src_nz; - - double d = fabs(tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)); - - double compare_mul = sqrt(dst_noise*dst_noise + point_noise*point_noise); - d *= compare_mul; - - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - - if(d < 0.01 && surface_angle > 0.5){//If close, according noises, and angle of the surfaces similar: FUSE - float px = m00*dst_x + m01*dst_y + m02*dst_z + m03; - float py = m10*dst_x + m11*dst_y + m12*dst_z + m13; - float pz = m20*dst_x + m21*dst_y + m22*dst_z + m23; - - float pnx = m00*dst_nx + m01*dst_ny + m02*dst_nz; - float pny = m10*dst_nx + m11*dst_ny + m12*dst_nz; - float pnz = m20*dst_nx + m21*dst_ny + m22*dst_nz; - - float pb = (float)(rgbdata[3*dst_ind+0]); - float pg = (float)(rgbdata[3*dst_ind+1]); - float pr = (float)(rgbdata[3*dst_ind+2]); - - Vector3f pxyz (px ,py ,pz ); - Vector3f pnxyz (pnx,pny,pnz); - Vector3f prgb (pr ,pg ,pb ); - float weight = 1.0/(dst_noise*dst_noise); - superpoint sp2 = superpoint(pxyz,pnxyz,prgb, weight, weight, frameid); - sp.merge(sp2); - isfused[dst_ind] = true; - } - } - } } +} - int nr_fused = 0; - int nr_mask = 0; - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - nr_fused += isfused[ind]; - nr_mask += maskvec[ind] > 0; - } +void Model::addAllSuperPoints(std::vector & spvec, Eigen::Matrix4d pose, boost::shared_ptr viewer){ + for(unsigned int i = 0; i < frames.size(); i++){ + addSuperPoints(spvec, pose*relativeposes[i], frames[i], modelmasks[i], viewer); } - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - if(!isfused[ind] && maskvec[ind]){ - float z = idepth*float(depthdata[ind]); - float nx = normalsdata[3*ind+0]; - - if(z > 0 && nx != 2){ - float ny = normalsdata[3*ind+1]; - float nz = normalsdata[3*ind+2]; - - float x = (w - cx) * z * ifx; - float y = (h - cy) * z * ify; - - double noise = z * z; - float px = m00*x + m01*y + m02*z + m03; - float py = m10*x + m11*y + m12*z + m13; - float pz = m20*x + m21*y + m22*z + m23; - float pnx = m00*nx + m01*ny + m02*nz; - float pny = m10*nx + m11*ny + m12*nz; - float pnz = m20*nx + m21*ny + m22*nz; - - float pb = rgbdata[3*ind+0]; - float pg = rgbdata[3*ind+1]; - float pr = rgbdata[3*ind+2]; - - Vector3f pxyz (px ,py ,pz ); - Vector3f pnxyz (pnx,pny,pnz); - Vector3f prgb (pr ,pg ,pb ); - float weight = 1.0/(noise*noise); - spvec.push_back(superpoint(pxyz,pnxyz,prgb, weight, weight, frameid)); - } - } - } + for(unsigned int i = 0; i < submodels.size(); i++){ + submodels[i]->addAllSuperPoints(spvec, pose*submodels_relativeposes[i], viewer); } +} -// if(viewer != 0){ -//// pcl::PointCloud::Ptr framecloud = getPointCloudFromVector(framesp); -//// viewer->removeAllPointClouds(); -//// viewer->addPointCloud (framecloud, pcl::visualization::PointCloudColorHandlerRGBField(framecloud), "framecloud"); -//// viewer->spin(); - -// viewer->removeAllPointClouds(); -// pcl::PointCloud::Ptr cloud = getPCLcloud(1, false); -// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "model"); -// viewer->spin(); -// } - //delete[] isfused; - */ +void Model::recomputeModelPoints(Eigen::Matrix4d pose, boost::shared_ptr viewer){ + points.clear(); + addAllSuperPoints(points,pose,viewer); } void Model::showHistory(boost::shared_ptr viewer){ @@ -503,21 +304,6 @@ std::vector::Ptr> ret; return ret; } -void Model::addAllSuperPoints(std::vector & spvec, Eigen::Matrix4d pose, boost::shared_ptr viewer){ - for(unsigned int i = 0; i < frames.size(); i++){ - addSuperPoints(spvec, pose*relativeposes[i], frames[i], modelmasks[i], viewer); - } - - for(unsigned int i = 0; i < submodels.size(); i++){ - submodels[i]->addAllSuperPoints(spvec, pose*submodels_relativeposes[i], viewer); - } -} - -void Model::recomputeModelPoints(Eigen::Matrix4d pose, boost::shared_ptr viewer){ - points.clear(); - addAllSuperPoints(points,pose,viewer); -} - void Model::addPointsToModel(RGBDFrame * frame, ModelMask * modelmask, Eigen::Matrix4d p){ bool * maskvec = modelmask->maskvec; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 5fa95499..96d564a6 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1937,7 +1937,83 @@ double normalCFD(double value) } void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ + Camera * camera = frame1->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + unsigned long nr_pixels = width*height; + std::vector framesp1 = frame1->getSuperPoints(p); + std::vector framesp2 = frame2->getSuperPoints(p); +// std::vector rr_vec = frame1->getReprojections(spvec,p.inverse(),0,true); +// std::vector framesp = frame->getSuperPoints(p); + +// unsigned long nr_rr = rr_vec.size(); + + printf("getDynamicWeights\n");// nr_rr: %i\n",nr_rr); + exit(0); + /* + if(nr_rr > 10){ + std::vector residualsZ; + double stdval = 0; + for(unsigned long ind = 0; ind < nr_rr;ind++){ + ReprojectionResult & rr = rr_vec[ind]; + superpoint & src_p = spvec[rr.src_ind]; + superpoint & dst_p = framesp[rr.dst_ind]; + double src_variance = 1.0/src_p.point_information; + double dst_variance = 1.0/dst_p.point_information; + double total_variance = src_variance+dst_variance; + double total_stdiv = sqrt(total_variance); + double d = rr.residualZ; + residualsZ.push_back(d/total_stdiv); + stdval += residualsZ.back()*residualsZ.back(); + } + stdval = sqrt(stdval/double(nr_rr)); + + DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + func->zeromean = true; + func->maxp = 0.99; + func->startreg = 0.001; + func->debugg_print = false; + func->maxd = 0.1; + func->startmaxd = func->maxd; + func->histogram_size = 100; + func->starthistogram_size = func->histogram_size; + func->stdval2 = stdval; + func->maxnoise = stdval; + func->reset(); + ((DistanceWeightFunction2*)func)->computeModel(residualsZ); + + for(unsigned long ind = 0; ind < nr_rr;ind++){ + double p = func->getProb(residualsZ[ind]); + if(p > 0.5){sumw[rr_vec[ind].dst_ind] += p;} + } + + for(unsigned long ind = 0; ind < nr_rr;ind++){ + double p = func->getProb(residualsZ[ind]); + if(p > 0.5){ + ReprojectionResult & rr = rr_vec[ind]; + superpoint & src_p = spvec[rr.src_ind]; + superpoint & dst_p = framesp[rr.dst_ind]; + float weight = p/sumw[rr.dst_ind]; + src_p.merge(dst_p,weight); + } + } + delete func; + } + + for(unsigned long ind = 0; ind < nr_pixels;ind++){ + if(maskvec[ind] != 0 && sumw[ind] < 0.5){spvec.push_back(framesp[ind]);} + } + + if(viewer != 0){ + pcl::PointCloud::Ptr cloud = getPointCloudFromVector(spvec,1); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "model"); + viewer->spin(); + viewer->removeAllPointClouds(); + } +*/ + +/* unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); float * src_normalsdata = (float *)(frame1->normals.data); @@ -2086,50 +2162,15 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / sqrt(dst_noise*dst_noise + point_noise*point_noise); -// if(offset1 >= 0 && offset2 >= 0){ -// if(fabs(d) < 0.02){ -// interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); -// interframe_connectionStrength[offset1+src_ind].push_back(2); -// } -// } - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; -// if(dst_detdata[dst_ind] != 0){continue;} -// if(src_detdata[src_ind] != 0){continue;} - - d = fabs(d); + d = fabs(d); if(tz > dst_z){d = -d;} if(store_distance){ dvec.push_back(d); nvec.push_back(1-surface_angle); }else{ - - -// double noise = 0.02; -// double noise_angle = 0.05; -// double p_overlap = exp(-0.5*(d*d)/(noise*noise)); -// double p_overlap_angle = exp(-0.5*((1-surface_angle)*(1-surface_angle))/(noise_angle*noise_angle)); -// p_overlap = std::min(0.9,p_overlap); -// double p_occlusion = (1.0-p_overlap)*normalCFD(d/noise); -// p_overlap *= p_overlap_angle; - -// if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0){ -// interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); -// interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap)); -// } -// if(debugg){ -// src_cloud->points[src_ind].r = 255-255.0 * func_overlap_angle; -// src_cloud->points[src_ind].g = 255.0 * func_overlap_angle; -// src_cloud->points[src_ind].b = 0; -// } -// if(dst_detdata[dst_ind] != 0){continue;} -// if(src_detdata[src_ind] != 0){continue;} - -// overlaps[src_ind] = std::max(overlaps[src_ind],p_overlap); -// occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); - double p_overlap_angle = nfunc->getProb(1-surface_angle); double p_overlap = dfunc->getProb(d); double p_occlusion = dfunc->getProbInfront(d); @@ -2147,33 +2188,12 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & src_cloud->points[src_ind].b = 0; } -// if(debugg){ -// src_cloud->points[src_ind].r = 255-255.0 * func_overlap_angle; -// src_cloud->points[src_ind].g = 255.0 * func_overlap_angle; -// src_cloud->points[src_ind].b = 0; -// } if(dst_detdata[dst_ind] != 0){continue;} if(src_detdata[src_ind] != 0){continue;} overlaps[src_ind] = std::min(0.9,std::max(overlaps[src_ind],p_overlap)); occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); } -// if(fabs(d) < 0.02){//If close, according noises, and angle of the surfaces similar: FUSE -// if(surface_angle > 0.9){ -// overlaps[src_ind] = std::max(overlaps[src_ind],0.9); -// if(offset1 >= 0 && offset2 >= 0){ -// interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); -// interframe_connectionStrength[offset1+src_ind].push_back(-log(1-overlaps[src_ind])); -// } -// } -// }else if(tz < dst_z){ -// occlusions[src_ind] = std::max(occlusions[src_ind],0.9); -// if(debugg){ -// src_cloud->points[src_ind].r = 255; -// src_cloud->points[src_ind].g = 0; -// src_cloud->points[src_ind].b = 255; -// } -// } } } } @@ -2187,6 +2207,7 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); viewer->spin(); } +*/ } void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ @@ -2212,8 +2233,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s float * priors = new float[3*tot_nr_pixels]; bool * valids = new bool[tot_nr_pixels]; - //bool store_distance = true; - std::vector dvec; std::vector nvec; DistanceWeightFunction2 * dfunc; diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 1e6fb371..59b7e2fb 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -184,20 +184,20 @@ void MassRegistrationPPR2::addModel(Model * model){ Eigen::VectorXd information (count); for(unsigned long c = 0; c < count; c++){ + superpoint & sp = model->points[c*step]; + ap[3*c+0] = sp.point(0); + ap[3*c+1] = sp.point(1); + ap[3*c+2] = sp.point(2); - ap[3*c+0] = model->points[c*step].point(0); - ap[3*c+1] = model->points[c*step].point(1); - ap[3*c+2] = model->points[c*step].point(2); + an[3*c+0] = sp.normal(0); + an[3*c+1] = sp.normal(1); + an[3*c+2] = sp.normal(2); - an[3*c+0] = model->points[c*step].normal(0); - an[3*c+1] = model->points[c*step].normal(1); - an[3*c+2] = model->points[c*step].normal(2); + ac[3*c+0] = sp.feature(0); + ac[3*c+1] = sp.feature(1); + ac[3*c+2] = sp.feature(2); - ac[3*c+0] =model->points[c*step].feature(0); - ac[3*c+1] =model->points[c*step].feature(1); - ac[3*c+2] =model->points[c*step].feature(2); - - ai[c] = 1.0/model->points[c*step].point_information;//1.0/(z*z); + ai[c] = sqrt(1.0/sp.point_information);//1.0/(z*z); X(0,c) = ap[3*c+0]; X(1,c) = ap[3*c+1]; @@ -210,6 +210,33 @@ void MassRegistrationPPR2::addModel(Model * model){ C(0,c) = ac[3*c+0]; C(1,c) = ac[3*c+1]; C(2,c) = ac[3*c+2]; + + +// ap[3*c+0] = model->points[c*step].point(0); +// ap[3*c+1] = model->points[c*step].point(1); +// ap[3*c+2] = model->points[c*step].point(2); + +// an[3*c+0] = model->points[c*step].normal(0); +// an[3*c+1] = model->points[c*step].normal(1); +// an[3*c+2] = model->points[c*step].normal(2); + +// ac[3*c+0] =model->points[c*step].feature(0); +// ac[3*c+1] =model->points[c*step].feature(1); +// ac[3*c+2] =model->points[c*step].feature(2); + +// ai[c] = 1.0/model->points[c*step].point_information;//1.0/(z*z); + +// X(0,c) = ap[3*c+0]; +// X(1,c) = ap[3*c+1]; +// X(2,c) = ap[3*c+2]; +// Xn(0,c) = an[3*c+0]; +// Xn(1,c) = an[3*c+1]; +// Xn(2,c) = an[3*c+2]; + +// information(c) = ai[c];//1.0/(z*z); +// C(0,c) = ac[3*c+0]; +// C(1,c) = ac[3*c+1]; +// C(2,c) = ac[3*c+2]; } informations[i] = information; @@ -266,7 +293,7 @@ void MassRegistrationPPR2::addModel(Model * model){ depthedge_trees3d[i]->buildIndex(); } */ - printf("total load time: %5.5f\n",getTime()-total_load_time_start); + printf("addModel total load time: %5.5f points: %6.6i\n",getTime()-total_load_time_start,count); } void MassRegistrationPPR2::addModelData(Model * model_, bool submodels){ From a4163fc3f4ac972f6f839a382d8d3b44b6703251 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 13 Sep 2016 23:00:36 +0200 Subject: [PATCH 103/255] working on ppr --- .../quasimodo_models/include/core/Util.h | 2 +- .../DistanceWeightFunction2PPR2.h | 1 + .../quasimodo_models/src/core/RGBDFrame.cpp | 1 + quasimodo/quasimodo_models/src/core/Util.cpp | 9 +- .../src/modelupdater/ModelUpdater.cpp | 559 +++++++++++++----- .../DistanceWeightFunction2PPR2.cpp | 341 +++++++---- 6 files changed, 633 insertions(+), 280 deletions(-) diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index 0f4f88be..7d052797 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -76,7 +76,7 @@ inline double getInformation(double depth){ return 1.0/(n*n); } -pcl::PointCloud::Ptr getPointCloudFromVector(std::vector & spvec, int colortype = 0); +pcl::PointCloud::Ptr getPointCloudFromVector(std::vector & spvec, int colortype = 0, int r = -1, int g = -1, int b = -1); /** diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h index 703f21d8..c135a77c 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h @@ -64,6 +64,7 @@ class DistanceWeightFunction2PPR2 : public DistanceWeightFunction2 double startmaxd; double maxd; + double mind; int histogram_size; int starthistogram_size; double blurval; diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 0ef3cf3b..9fd2e3ed 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -2265,6 +2265,7 @@ std::vector RGBDFrame::getReprojections(std::vector::Ptr getPointCloudFromVector(std::vector & spvec, int colortype){ + pcl::PointCloud::Ptr getPointCloudFromVector(std::vector & spvec, int colortype, int r, int g, int b){ unsigned long nr_points = spvec.size(); pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); cloud_ptr->width = nr_points; @@ -28,6 +28,13 @@ namespace reglib{ p0.g = 255.0*std::min(1.0,std::max(0.0,1.0-sqrt(p.point_information))); p0.b = 0; } + if(colortype == 3){ + if(r >= 0){ + p0.r = r; + p0.g = g; + p0.b = b; + } + } p0.normal_x = p.normal(0); p0.normal_y = p.normal(1); p0.normal_z = p.normal(2); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 96d564a6..9dc4330f 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1933,23 +1933,282 @@ std::vector doInference(std::vector & prior, std::vector< std::vect double normalCFD(double value) { - return 0.5 * erfc(-value * M_SQRT1_2); + return 0.5 * erfc(-value * M_SQRT1_2); } void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ - Camera * camera = frame1->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - unsigned long nr_pixels = width*height; - std::vector framesp1 = frame1->getSuperPoints(p); - std::vector framesp2 = frame2->getSuperPoints(p); -// std::vector rr_vec = frame1->getReprojections(spvec,p.inverse(),0,true); -// std::vector framesp = frame->getSuperPoints(p); - -// unsigned long nr_rr = rr_vec.size(); - - printf("getDynamicWeights\n");// nr_rr: %i\n",nr_rr); + // Camera * camera = frame1->camera; + // const unsigned int width = camera->width; + // const unsigned int height = camera->height; + // unsigned long nr_pixels = width*height; + + unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); + unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); + + std::vector framesp1 = frame1->getSuperPoints(p); + std::vector framesp2 = frame2->getSuperPoints();//p); + std::vector rr_vec = frame2->getReprojections(framesp1,Eigen::Matrix4d::Identity(),0,false); + + + unsigned long nr_rr = rr_vec.size(); + printf("getDynamicWeights nr_rr: %i\n",nr_rr); + + pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); + if(debugg){ + src_cloud = getPointCloudFromVector(framesp1,3,255,0,255); + dst_cloud = getPointCloudFromVector(framesp2,3,0,0,255); + } + + int totsum = 0; + for(unsigned long ind = 0; ind < nr_rr;ind++){ + ReprojectionResult & rr = rr_vec[ind]; + unsigned int src_ind = rr.src_ind; + superpoint & src_p = framesp1[src_ind]; + if(src_p.point(2) < 0.0){continue;} + + totsum++; + unsigned int dst_ind = rr.dst_ind; + superpoint & dst_p = framesp2[dst_ind]; + double src_variance = 1.0/src_p.point_information; + double dst_variance = 1.0/dst_p.point_information; + double total_variance = src_variance+dst_variance; + double total_stdiv = sqrt(total_variance); + double d = rr.residualZ/total_stdiv; + double surface_angle = rr.angle; + if(store_distance){ + dvec.push_back(d); + nvec.push_back(1-surface_angle); + }else{ + double p_overlap_angle = nfunc->getProb(1-surface_angle); + double p_overlap = dfunc->getProb(d); + double p_occlusion = dfunc->getProbInfront(d); + + p_overlap *= p_overlap_angle; + + if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0){ + interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); + interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap)); + } + + if(debugg){ + src_cloud->points[src_ind].r = 255.0 * p_overlap; + src_cloud->points[src_ind].g = 255.0 * p_occlusion; + src_cloud->points[src_ind].b = 0; + } + + if(dst_detdata[dst_ind] != 0){continue;} + if(src_detdata[src_ind] != 0){continue;} + + overlaps[src_ind] = std::min(0.9,std::max(overlaps[src_ind],p_overlap)); + occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); + } + } + + if(debugg && totsum > 0){ + pcl::PointCloud::Ptr src_cloud = getPointCloudFromVector(framesp1,3,255,0,255); + pcl::PointCloud::Ptr dst_cloud = getPointCloudFromVector(framesp2,3,0,0,255); + viewer->removeAllPointClouds(); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); + viewer->spin(); + } + + // std::vector residualsZ; + // double stdval = 0; + // residualsZ.push_back(d); + // stdval += residualsZ.back()*residualsZ.back(); + // stdval = sqrt(stdval/double(nr_rr)); + + // DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + // func->zeromean = true; + // func->maxp = 0.99; + // func->startreg = 0.0001; + // func->debugg_print = true; + // func->maxd = 0.1; + // func->startmaxd = func->maxd; + // func->histogram_size = 100; + // func->starthistogram_size = func->histogram_size; + // func->stdval2 = stdval; + // func->maxnoise = stdval; + // func->reset(); + // ((DistanceWeightFunction2*)func)->computeModel(residualsZ); + + + // double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; + + // d = fabs(d); + // if(tz > dst_z){d = -d;} + // if(store_distance){ + // dvec.push_back(d); + // nvec.push_back(1-surface_angle); + // }else{ + + // double p_overlap_angle = nfunc->getProb(1-surface_angle); + // double p_overlap = dfunc->getProb(d); + // double p_occlusion = dfunc->getProbInfront(d); + + // p_overlap *= p_overlap_angle; + + // if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0){ + // interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); + // interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap)); + // } + + // if(debugg){ + // src_cloud->points[src_ind].r = 255.0 * p_overlap; + // src_cloud->points[src_ind].g = 255.0 * p_occlusion; + // src_cloud->points[src_ind].b = 0; + // } + + // if(dst_detdata[dst_ind] != 0){continue;} + // if(src_detdata[src_ind] != 0){continue;} + + // overlaps[src_ind] = std::min(0.9,std::max(overlaps[src_ind],p_overlap)); + // occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); + // } + // } + //} + + // for(unsigned long ind = 0; ind < nr_rr;ind++){ + // double p = func->getProb(residualsZ[ind]); + // if(p > 0.5){sumw[rr_vec[ind].dst_ind] += p;} + // } + + // for(unsigned long ind = 0; ind < nr_rr;ind++){ + // double p = func->getProb(residualsZ[ind]); + // if(p > 0.5){ + // ReprojectionResult & rr = rr_vec[ind]; + // superpoint & src_p = spvec[rr.src_ind]; + // superpoint & dst_p = framesp[rr.dst_ind]; + // float weight = p/sumw[rr.dst_ind]; + // src_p.merge(dst_p,weight); + // } + // } + //delete func; + + // for(unsigned long ind = 0; ind < nr_pixels;ind++){ + // if(maskvec[ind] != 0 && sumw[ind] < 0.5){ + // superpoint & sp = framesp[ind]; + // if(sp.point_information > 0){ + // spvec.push_back(sp); + // } + // } + // } + + + /* + + + unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); + unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); + float * src_normalsdata = (float *)(frame1->normals.data); + + + unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); + unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); + + Camera * src_camera = frame1->camera; + const unsigned int src_width = src_camera->width; + const unsigned int src_height = src_camera->height; + const float src_idepth = src_camera->idepth_scale; + const float src_cx = src_camera->cx; + const float src_cy = src_camera->cy; + const float src_ifx = 1.0/src_camera->fx; + const float src_ify = 1.0/src_camera->fy; + + unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); + unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); + float * dst_normalsdata = (float *)(frame2->normals.data); + + Camera * dst_camera = frame2->camera; + const unsigned int dst_width = dst_camera->width; + const unsigned int dst_height = dst_camera->height; + const float dst_idepth = dst_camera->idepth_scale; + const float dst_cx = dst_camera->cx; + const float dst_cy = dst_camera->cy; + const float dst_ifx = 1.0/dst_camera->fx; + const float dst_ify = 1.0/dst_camera->fy; + const float dst_fx = dst_camera->fx; + const float dst_fy = dst_camera->fy; + + const unsigned int dst_width2 = dst_camera->width - 2; + const unsigned int dst_height2 = dst_camera->height - 2; + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + //bool debugg = true; + pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); + if(debugg){ + src_cloud->points.resize(src_width*src_height); + for(unsigned int src_w = 0; src_w < src_width;src_w++){ + for(unsigned int src_h = 0; src_h < src_height;src_h++){ + int src_ind = src_h*src_width+src_w; + float src_z = src_idepth*float(src_depthdata[src_ind]); + float src_nx = src_normalsdata[3*src_ind+0]; + if(src_z > 0 && src_nx != 2){ + float src_ny = src_normalsdata[3*src_ind+1]; + float src_nz = src_normalsdata[3*src_ind+2]; + + float src_x = (float(src_w) - src_cx) * src_z * src_ifx; + float src_y = (float(src_h) - src_cy) * src_z * src_ify; + + + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; + + pcl::PointXYZRGBNormal point; + point.x = tx; + point.y = ty; + point.z = tz; + point.r = 0; + point.g = 0; + point.b = 0; + src_cloud->points[src_ind] = point; + } + } + } + + dst_cloud->points.resize(dst_width*dst_height); + for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ + for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ + int dst_ind = dst_h*dst_width+dst_w; + float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); + float dst_nx = dst_normalsdata[3*dst_ind+0]; + if(dst_z > 0 && dst_nx != 2){ + float dst_ny = dst_normalsdata[3*dst_ind+1]; + float dst_nz = dst_normalsdata[3*dst_ind+2]; + + float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; + float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; + + pcl::PointXYZRGBNormal point; + point.x = dst_x; + point.y = dst_y; + point.z = dst_z; + point.r = 255; + point.g = 0; + point.b = 255; + dst_cloud->points[dst_ind] = point; + } + } + } + } + + if(debugg){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); + viewer->spin(); + } + + exit(0); + */ /* if(nr_rr > 10){ std::vector residualsZ; @@ -2013,7 +2272,7 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & */ -/* + /* unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); float * src_normalsdata = (float *)(frame1->normals.data); @@ -2145,7 +2404,7 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; + float dst_nx = dst_normalsdata[3*dst_ind+0]; if(dst_z > 0 && dst_nx != 2){ float dst_ny = dst_normalsdata[3*dst_ind+1]; float dst_nz = dst_normalsdata[3*dst_ind+2]; @@ -2165,7 +2424,7 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; d = fabs(d); - if(tz > dst_z){d = -d;} + if(tz > dst_z){d = -d;} if(store_distance){ dvec.push_back(d); nvec.push_back(1-surface_angle); @@ -2265,13 +2524,15 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dfunc = dfuncTMP; dfuncTMP->refine_mean = true; dfuncTMP->zeromean = false; - dfuncTMP->startreg = 0.0005; - dfuncTMP->debugg_print = false; + //dfuncTMP->startreg = 0.0005; + dfuncTMP->startreg = 0.0; + dfuncTMP->debugg_print = true; dfuncTMP->bidir = true; + //dfuncTMP->bidir = false; dfuncTMP->maxp = 0.9; - dfuncTMP->maxd = 0.03; + dfuncTMP->maxd = 0.5; dfuncTMP->histogram_size = 1000; - dfuncTMP->fixed_histogram_size = true; + dfuncTMP->fixed_histogram_size = false;//true; dfuncTMP->max_under_mean = false; dfuncTMP->startmaxd = dfuncTMP->maxd; dfuncTMP->starthistogram_size = dfuncTMP->histogram_size; @@ -2302,7 +2563,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s nfuncTMP->reset(); nfunc->computeModel(nvec); - +//exit(0); int frameConnections = 0; std::vector< std::vector< std::vector > > pixel_weights; @@ -2372,7 +2633,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s float leftover = 1-p_moving-p_dynamic-p_static; float notMoving = current_overlaps[ind]; - float bias = 0.0001; + float bias = 0.0001; @@ -2380,27 +2641,27 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; -// float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover); -// float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover); -// float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover); + // float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover); + // float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover); + // float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover); float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover) + minprob; float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover) + minprob; float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover) + 2.0*minprob; - priors[3*(offset+ind)+0] = p_moving_tot; - priors[3*(offset+ind)+1] = p_dynamic_tot; - priors[3*(offset+ind)+2] = p_static_tot; + priors[3*(offset+ind)+0] = p_moving_tot; + priors[3*(offset+ind)+1] = p_dynamic_tot; + priors[3*(offset+ind)+2] = p_static_tot; -// float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; -// float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; -// float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; + // float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; + // float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; + // float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; -// priors[3*(offset+ind)+0] = p_moving_tot; -// priors[3*(offset+ind)+1] = p_dynamic_tot; -// priors[3*(offset+ind)+2] = p_static_tot; + // priors[3*(offset+ind)+0] = p_moving_tot; + // priors[3*(offset+ind)+1] = p_dynamic_tot; + // priors[3*(offset+ind)+2] = p_static_tot; if(probs[0][ind] > 0.00000001){frameConnections++;} if(probs[1][ind] > 0.00000001){frameConnections++;} @@ -2423,9 +2684,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s point.x = m00*x + m01*y + m02*z + m03; point.y = m10*x + m11*y + m12*z + m13; point.z = m20*x + m21*y + m22*z + m23; - point.r = priors[3*(offset+ind)+0]*255.0; - point.g = priors[3*(offset+ind)+1]*255.0; - point.b = priors[3*(offset+ind)+2]*255.0; + point.r = priors[3*(offset+ind)+0]*255.0; + point.g = priors[3*(offset+ind)+1]*255.0; + point.b = priors[3*(offset+ind)+2]*255.0; cloud->points.push_back(point); } } @@ -2453,12 +2714,12 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s gc::Graph * g = new gc::Graph(current_point,frameConnections+interframeConnections); for(unsigned long i = 0; i < current_point;i++){ g -> add_node(); - double p_fg = priors[3*i+0]+priors[3*i+1]; - double p_bg = priors[3*i+2]; + double p_fg = priors[3*i+0]+priors[3*i+1]; + double p_bg = priors[3*i+2]; double norm = p_fg + p_bg; p_fg /= norm; p_bg /= norm; - if(priors[3*i+0]+priors[3*i+1]+priors[3*i+2] <= 0){ + if(priors[3*i+0]+priors[3*i+1]+priors[3*i+2] <= 0){ g -> add_tweights( i, 0, 0 ); continue; } @@ -2468,7 +2729,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s g -> add_tweights( i, weightFG, weightBG ); } - double maxprob_same = 0.99999999999; + double maxprob_same = 0.99999999999; for(unsigned int i = 0; i < frames.size(); i++){ int offset = offsets[i]; Camera * camera = frames[i]->camera; @@ -2480,7 +2741,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s int ind = h*width+w; if(w > 0 && probs[0][ind] > 0.00000001){ int other = ind-1; - double p_same = std::min(double(probs[0][ind]),maxprob_same); + double p_same = std::min(double(probs[0][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); g -> add_edge( ind+offset, other+offset, weight, weight ); @@ -2488,7 +2749,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s if(h > 0 && probs[1][ind] > 0.00000001){ int other = ind-width; - double p_same = std::min(double(probs[1][ind]),maxprob_same); + double p_same = std::min(double(probs[1][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); g -> add_edge( ind+offset, other+offset, weight, weight ); @@ -2569,8 +2830,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s gc::Graph * dynamic_g = new gc::Graph(nr_dynamic,dynamic_frameConnections+dynamic_interframeConnections); for(unsigned long i = 0; i < dyn_ind.size();i++){ dynamic_g -> add_node(); - double p_fg = priors[3*dyn_ind[i]+0]; - double p_bg = priors[3*dyn_ind[i]+1]; + double p_fg = priors[3*dyn_ind[i]+0]; + double p_bg = priors[3*dyn_ind[i]+1]; double norm = p_fg+p_bg; p_fg /= norm; p_bg /= norm; @@ -2598,7 +2859,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s int other = ind-1; if(labels[ind+offset] == labels[other+offset]){ //dynamic_frameConnections++; - double p_same = std::min(double(probs[0][ind]),maxprob_same); + double p_same = std::min(double(probs[0][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight ); @@ -2609,7 +2870,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s int other = ind-width; if(labels[ind+offset] == labels[other+offset]){ //dynamic_frameConnections++; - double p_same = std::min(double(probs[0][ind]),maxprob_same); + double p_same = std::min(double(probs[0][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight ); @@ -2699,29 +2960,29 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s end_inf = getTime(); printf("dynamic cluster time: %10.10fs\n",end_inf-start_inf); for (unsigned int d = 0; d < dynamic_indices.size(); d++){ - double score = 0; + double score = 0; pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); pcl::PointCloud::Ptr cloud_cluster2 (new pcl::PointCloud); - double totsum = 0; + double totsum = 0; for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ int pid = dynamic_indices[d].indices[ind]; int dind = dynamicdata[pid]; - double p0 = priors[3*dind+0]; - double p1 = priors[3*dind+1]; - double p2 = priors[3*dind+2]; + double p0 = priors[3*dind+0]; + double p1 = priors[3*dind+1]; + double p2 = priors[3*dind+2]; double s = 0; if(valids[dind]){ - if(p0 > p2){s += p1 - p0;} - else{ s += p1 - p2;} - score += s; + if(p0 > p2){s += p1 - p0;} + else{ s += p1 - p2;} + score += s; totsum++; } - pcl::PointXYZRGBNormal p = cloud->points[dind]; - p.r = 0;p.g = 0;p.b = 0; + pcl::PointXYZRGBNormal p = cloud->points[dind]; + p.r = 0;p.g = 0;p.b = 0; if(s > 0){p.g = 255.0*s;} if(s < 0){p.r = -255.0*s;} @@ -2730,7 +2991,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } printf("score: %f avg: %f\n",score,score/totsum); - if(score > 200){ + if(score > 200){ for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ labels[dynamicdata[dynamic_indices[d].indices[ind]]] = 3; } @@ -2739,7 +3000,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s if(false && cloud_cluster->points.size() > 500){ viewer->removeAllPointClouds(); viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); - viewer->spin(); + viewer->spin(); viewer->removeAllPointClouds(); viewer->addPointCloud (cloud_cluster2, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster2), "cloud_cluster"); @@ -2763,93 +3024,93 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s end_inf = getTime(); printf("moving cluster time: %10.10fs\n",end_inf-start_inf); - for (unsigned int d = 0; d < moving_indices.size(); d++){ - double score = 0; - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_cluster2 (new pcl::PointCloud); - - double totsum = 0; - double sum0 = 0; - double sum1 = 0; - double sum2 = 0; - for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ - int pid = moving_indices[d].indices[ind]; - int dind = movingdata[pid]; - - double p0 = priors[3*dind+0]; - double p1 = priors[3*dind+1]; - double p2 = priors[3*dind+2]; - - double s = 0; - if(valids[dind]){ - if(p1 > p2){s += p0 - p1;} - else{ s += p0 - p2;} - score += s; - totsum++; - sum0 += p0; - sum1 += p1; - sum2 += p2; - } - - pcl::PointXYZRGBNormal p = cloud->points[dind]; - p.r = 0;p.g = 0;p.b = 0; - if(s > 0){p.g = 255.0*s;} - if(s < 0){p.r = -255.0*s;} - - cloud_cluster->points.push_back(movingcloud->points[pid]); - cloud_cluster2->points.push_back(p); - } - -// printf("score: %f avg: %f\n",score,score/totsum); -// printf("sum: %f %f %f\n",sum0,sum1,sum2); - if(score > 200){ - for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ - labels[movingdata[moving_indices[d].indices[ind]]] = 4; - } - } - - if(false && cloud_cluster->points.size() > 500){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); - viewer->spin(); - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_cluster2, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster2), "cloud_cluster"); - viewer->spin(); - } - - -// double score = 0; -// pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); -// double sum0 = 0; -// double sum1 = 0; -// double sum2 = 0; -// for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ -// int pid = moving_indices[d].indices[ind]; -// int dind = movingdata[pid]; -// score += priors[3*dind+0] -0.5*(priors[3*dind+1]+priors[3*dind+2]); - -// sum0 += priors[3*dind+0]; -// sum1 += priors[3*dind+1]; -// sum2 += priors[3*dind+2]; - -// cloud_cluster->points.push_back(movingcloud->points[pid]); -// } - -// double avg = score/double(moving_indices[d].indices.size()); -//// printf("score: %f avg: %f\n",score,avg); -//// printf("sum0: %f sum1: %f sum2: %f\n",sum0,sum1,sum2); -// if(score > 100){ -// for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ -// labels[movingdata[moving_indices[d].indices[ind]]] = 4; -// } -// } - -//// if(cloud_cluster->points.size() > 500){ -//// viewer->removeAllPointClouds(); -//// viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); -//// viewer->spin(); -//// } + for (unsigned int d = 0; d < moving_indices.size(); d++){ + double score = 0; + pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud_cluster2 (new pcl::PointCloud); + + double totsum = 0; + double sum0 = 0; + double sum1 = 0; + double sum2 = 0; + for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ + int pid = moving_indices[d].indices[ind]; + int dind = movingdata[pid]; + + double p0 = priors[3*dind+0]; + double p1 = priors[3*dind+1]; + double p2 = priors[3*dind+2]; + + double s = 0; + if(valids[dind]){ + if(p1 > p2){s += p0 - p1;} + else{ s += p0 - p2;} + score += s; + totsum++; + sum0 += p0; + sum1 += p1; + sum2 += p2; + } + + pcl::PointXYZRGBNormal p = cloud->points[dind]; + p.r = 0;p.g = 0;p.b = 0; + if(s > 0){p.g = 255.0*s;} + if(s < 0){p.r = -255.0*s;} + + cloud_cluster->points.push_back(movingcloud->points[pid]); + cloud_cluster2->points.push_back(p); + } + + // printf("score: %f avg: %f\n",score,score/totsum); + // printf("sum: %f %f %f\n",sum0,sum1,sum2); + if(score > 200){ + for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ + labels[movingdata[moving_indices[d].indices[ind]]] = 4; + } + } + + if(false && cloud_cluster->points.size() > 500){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); + viewer->spin(); + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_cluster2, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster2), "cloud_cluster"); + viewer->spin(); + } + + + // double score = 0; + // pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); + // double sum0 = 0; + // double sum1 = 0; + // double sum2 = 0; + // for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ + // int pid = moving_indices[d].indices[ind]; + // int dind = movingdata[pid]; + // score += priors[3*dind+0] -0.5*(priors[3*dind+1]+priors[3*dind+2]); + + // sum0 += priors[3*dind+0]; + // sum1 += priors[3*dind+1]; + // sum2 += priors[3*dind+2]; + + // cloud_cluster->points.push_back(movingcloud->points[pid]); + // } + + // double avg = score/double(moving_indices[d].indices.size()); + //// printf("score: %f avg: %f\n",score,avg); + //// printf("sum0: %f sum1: %f sum2: %f\n",sum0,sum1,sum2); + // if(score > 100){ + // for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ + // labels[movingdata[moving_indices[d].indices[ind]]] = 4; + // } + // } + + //// if(cloud_cluster->points.size() > 500){ + //// viewer->removeAllPointClouds(); + //// viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); + //// viewer->spin(); + //// } } @@ -2953,7 +3214,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector ModelUpdater::computeDynamicObject(vector bgcp, vector 0 && w < width-1){ int other = ind-1; - double p_same = std::min(double(probs[0][ind]),maxprob_same); + double p_same = std::min(double(probs[0][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); if(!std::isnan(weight) && weight > 0){ @@ -3077,7 +3338,7 @@ vector ModelUpdater::computeDynamicObject(vector bgcp, vector 0 && h < height-1){ int other = ind-width; - double p_same = std::min(double(probs[1][ind]),maxprob_same); + double p_same = std::min(double(probs[1][ind]),maxprob_same); double not_p_same = 1-p_same; double weight = -log(not_p_same); diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp index 8361c15a..f485c97c 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp @@ -8,6 +8,7 @@ DistanceWeightFunction2PPR2::DistanceWeightFunction2PPR2( double maxd_, int hist regularization = 0.1; maxd = maxd_; + mind = 0; histogram_size = histogram_size_; starthistogram_size = histogram_size; blurval = 5; @@ -659,126 +660,162 @@ double DistanceWeightFunction2PPR2::getNoise(){return regularization+noiseval;}/ #include void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ - //debugg_print = false; - const unsigned int nr_data = mat.cols(); const int nr_dim = mat.rows(); - //if(debugg_print){printf("histogram_size: %i maxd: %f\n",histogram_size,maxd);exit(0);} -if(debugg_print){printf("\n################################################### ITER:%i ############################################################\n",iter);} -if(!fixed_histogram_size){ - if(first){ - first = false; - double sum = 0; - for(unsigned int j = 0; j < nr_data; j++){ - for(int k = 0; k < nr_dim; k++){sum += mat(k,j)*mat(k,j);} + if(debugg_print){printf("\n################################################### ITER:%i ############################################################\n",iter);} + if(!fixed_histogram_size){ + if(first){ + first = false; + double sum = 0; + for(unsigned int j = 0; j < nr_data; j++){ + for(int k = 0; k < nr_dim; k++){sum += mat(k,j)*mat(k,j);} + } + stdval2 = sqrt(sum / double(nr_data*nr_dim)); + if(debugg_print){printf("stdval2: %f\n",stdval2);} } - stdval2 = sqrt(sum / double(nr_data*nr_dim)); - if(debugg_print){printf("stdval2: %f\n",stdval2);} - } - if(update_size){ - maxd = fabs(meanval2) + (stdval2 + regularization)*target_length; - if(debugg_print){printf("maxd: %f\n",maxd);} - } + if(update_size){ + //maxd = fabs(meanval2) + (stdval2 + regularization)*target_length; + if(bidir){ + maxd = meanval2 + 2.0*(stdval2 + regularization)*target_length; + mind = meanval2 - 2.0*(stdval2 + regularization)*target_length; + }else{ + maxd = fabs(meanval2) + (stdval2 + regularization)*target_length; + mind = 0; + } + if(debugg_print){ + printf("maxd: %f\n",maxd); + printf("mind: %f\n",mind); + } + } - double nr_inside = 0; - for(unsigned int j = 0; j < nr_data; j++){ - for(int k = 0; k < nr_dim; k++){ - if(fabs(mat(k,j)) < maxd){nr_inside++;} + double nr_inside = 0; + for(unsigned int j = 0; j < nr_data; j++){ + for(int k = 0; k < nr_dim; k++){ + //if(fabs(mat(k,j)) < maxd){nr_inside++;} + double d = mat(k,j); + if(mind < d && d < maxd){nr_inside++;} + } } - } - if(update_size){ - nr_inside = std::min(nr_inside,double(nr_dim)*nr_inliers); - histogram_size = std::min(int(prob.size()),std::max(50,std::min(1000,int(0.5 + nr_inside/data_per_bin)))); - blurval = blur*double(histogram_size); + if(update_size){ + nr_inside = std::min(nr_inside,double(nr_dim)*nr_inliers); + int new_histogram_size = std::min(int(prob.size()),std::max(50,int(0.5 + nr_inside/data_per_bin))); + histogram_size = std::min(1000,new_histogram_size);//std::min(int(prob.size()),std::max(50,std::min(1000,int(0.5 + nr_inside/data_per_bin)))); + blurval = blur*double(histogram_size)*float(histogram_size)/float(new_histogram_size); - if(debugg_print){printf("nr_inside: %f histogram_size: %i blurval: %f\n",nr_inside,histogram_size,blurval);} + if(debugg_print){printf("nr_inside: %f histogram_size: %i blurval: %f\n",nr_inside,histogram_size,blurval);} + } } -} - histogram_mul = double(histogram_size)/maxd; - if(bidir){ - histogram_mul2 = double(histogram_size)/(2.0*maxd); - }else{ - histogram_mul2 = double(histogram_size)/maxd; - } + histogram_mul = double(histogram_size)/maxd; + histogram_mul2 = double(histogram_size)/(maxd-mind); +// if(bidir){ histogram_mul2 = double(histogram_size)/(2.0*maxd);} +// else{ histogram_mul2 = double(histogram_size)/maxd;} - if(debugg_print){printf("histogram_mul: %f histogram_size: %f maxd: %f\n",histogram_mul,double(histogram_size),maxd);} + if(debugg_print){printf("histogram_mul: %5.5f histogram_size: %5.5f maxd: %5.5f mind: %5.5f\n",histogram_mul,double(histogram_size),maxd,mind);} double start_time = getCurrentTime3(); for(int j = 0; j < histogram_size; j++){histogram[j] = 0;} - - if(!fixed_histogram_size){ - for(unsigned int j = 0; j < nr_data; j++){ - for(int k = 0; k < nr_dim; k++){ - double ind = fabs(mat(k,j))*histogram_mul; - double w2 = ind-int(ind); - double w1 = 1-w2; - if(ind >= 0 && (ind+0.5) < histogram_size){histogram[int(ind+0.5)]++;} - } - } - histogram[0]*=2; -}else if(bidir){ - for(unsigned int j = 0; j < histogram_size; j++){histogram[j] = 0;} - for(unsigned int j = 0; j < nr_data; j++){ - for(int k = 0; k < nr_dim; k++){ - double ind = double(histogram_size)*(mat(k,j)+maxd)/(2.0*maxd); - if(ind >= 0 && (ind+0.00001) < histogram_size){ - histogram[int(ind+0.00001)]++; - } - } - } -}else{ for(unsigned int j = 0; j < nr_data; j++){ for(int k = 0; k < nr_dim; k++){ - double ind = fabs(mat(k,j))*histogram_mul; - double w2 = ind-int(ind); - double w1 = 1-w2; + double ind = getInd(mat(k,j)); if(ind >= 0 && (ind+0.00001) < histogram_size){ histogram[int(ind+0.00001)]++; } - //if(j % 5000 == 0){printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind);} } } -} + + //double ind = getInd(d,debugg); + +// if(!fixed_histogram_size){ +// for(unsigned int j = 0; j < nr_data; j++){ +// for(int k = 0; k < nr_dim; k++){ +// double ind = fabs(mat(k,j))*histogram_mul; +// double w2 = ind-int(ind); +// double w1 = 1-w2; +// if(ind >= 0 && (ind+0.5) < histogram_size){histogram[int(ind+0.5)]++;} +// } +// } +// histogram[0]*=2; +//}else if(bidir){ +// for(unsigned int j = 0; j < histogram_size; j++){histogram[j] = 0;} +// for(unsigned int j = 0; j < nr_data; j++){ +// for(int k = 0; k < nr_dim; k++){ +// double ind = double(histogram_size)*(mat(k,j)+maxd)/(2.0*maxd); +// if(ind >= 0 && (ind+0.00001) < histogram_size){ +// histogram[int(ind+0.00001)]++; +// } +// } +// } +//}else{ +// for(unsigned int j = 0; j < nr_data; j++){ +// for(int k = 0; k < nr_dim; k++){ +// double ind = fabs(mat(k,j))*histogram_mul; +// double w2 = ind-int(ind); +// double w1 = 1-w2; +// if(ind >= 0 && (ind+0.00001) < histogram_size){ +// histogram[int(ind+0.00001)]++; +// } +// //if(j % 5000 == 0){printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind);} +// } +// } +//} start_time = getCurrentTime3(); blurHistogram3(blur_histogram,histogram,blurval,histogram_size,debugg_print); - stdval2 = 0; - Gaussian3 g = getModel(stdval, blur_histogram, uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); - Gaussian3 g2 = getModel(stdval2,histogram, uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); - stdval2 = std::max(1.0,stdval2); - stdval2 = maxd*stdval2/float(histogram_size); +// stdval2 = 0; +// Gaussian3 g = getModel(stdval, blur_histogram, uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); +// //Gaussian3 g2 = getModel(stdval2,histogram, uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); +// stdval2 = std::max(1.0,stdval2); +// stdval2 = maxd*stdval2/float(histogram_size); - g.stdval = std::max(0.0001*double(histogram_size)/maxd,std::min(g.stdval,maxnoise*double(histogram_size)/maxd)); +// g.stdval = std::max(0.0001*double(histogram_size)/(maxd),std::min(g.stdval,maxnoise*double(histogram_size)/maxd)); - noiseval = maxd*g.stdval/float(histogram_size); +// noiseval = maxd*g.stdval/float(histogram_size); +// stdval = g.stdval; - stdval = g.stdval; +// double Ib = 1/(blurval*blurval); +// double Ic = 1/(stdval*stdval); +// double Ia = Ib*Ic/(Ib-Ic); +// double corrected = sqrt(1.0/Ia); +// double rescaleval = 1; - double Ib = 1/(blurval*blurval); - double Ic = 1/(stdval*stdval); - double Ia = Ib*Ic/(Ib-Ic); - double corrected = sqrt(1.0/Ia); - double rescaleval = 1; +// if(rescaling){ +// if(stdval > blurval){ +// g.stdval = corrected; +// rescaleval = stdval/corrected; +// }else{g.stdval = 1;} +// } - if(rescaling){ - if(stdval > blurval){ - g.stdval = corrected; - rescaleval = stdval/corrected; - }else{g.stdval = 1;} - } +// stdval = g.stdval; +// mulval = g.mul; +// meanval = g.mean; + +// meanval2 = maxd*meanval/float(histogram_size); + +// g.stdval += histogram_size*regularization/maxd; +// g.update(); + + + Gaussian3 g = getModel(stdval, blur_histogram, uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); + g.stdval = std::max(0.0001*double(histogram_size)/(maxd),std::min(g.stdval,maxnoise*double(histogram_size)/maxd)); + + //noiseval = maxd*g.stdval/float(histogram_size); + noiseval = (maxd-mind)*g.stdval/float(histogram_size); + if(bidir){noiseval *= 0.5;} stdval = g.stdval; mulval = g.mul; meanval = g.mean; - meanval2 = maxd*meanval/float(histogram_size); + meanval2 = (maxd-mind)*(meanval/float(histogram_size)) + mind; + stdval2 = noiseval;//getNoise(); + //meanval2 = maxd*meanval/float(histogram_size); g.stdval += histogram_size*regularization/maxd; g.update(); @@ -788,28 +825,28 @@ if(!fixed_histogram_size){ for(int k = 0; k < histogram_size; k++){noisecdf[k] = g.getcdf(k);} } - std::vector new_histogram; - if(rescaling){ - new_histogram.resize(blur_histogram.size()); - double sum = 0; - for(int k = 0; k < histogram_size; k++){ - double ks = double(k)*rescaleval; - double w2 = ks-int(ks); - double w1 = 1-w2; - double newh = blur_histogram[histogram_size-1]; - if(ks < histogram_size){ - newh = blur_histogram[int(ks)]*w1 + blur_histogram[int(ks+1)]*w2; - sum += newh; - } - new_histogram[k] = newh; - } - } +// std::vector new_histogram; +// if(rescaling){ +// new_histogram.resize(blur_histogram.size()); +// double sum = 0; +// for(int k = 0; k < histogram_size; k++){ +// double ks = double(k)*rescaleval; +// double w2 = ks-int(ks); +// double w1 = 1-w2; +// double newh = blur_histogram[histogram_size-1]; +// if(ks < histogram_size){ +// newh = blur_histogram[int(ks)]*w1 + blur_histogram[int(ks+1)]*w2; +// sum += newh; +// } +// new_histogram[k] = newh; +// } +// } for(int k = 0; k < histogram_size; k++){ if(max_under_mean && k < g.mean){ prob[k] = maxp; } else{ double hs = blur_histogram[k]+1; - if(rescaling){hs = new_histogram[k]+1;} +// if(rescaling){hs = new_histogram[k]+1;} prob[k] = std::min(maxp , noise[k]/hs);//never fully trust any data infront[k] = (1-prob[k])*noisecdf[k]; } @@ -820,9 +857,10 @@ if(!fixed_histogram_size){ float inl = 1; float ninl = 1; for(int k = 0; k < nr_dim; k++){ - double ind = fabs(mat(k,j))*histogram_mul; - float p = 0; - if(ind >= 0 && (ind+0.5) < histogram_size){p = prob[int(ind+0.5)];} + //double ind = getInd(mat(k,j));//fabs(mat(k,j))*histogram_mul; + //float p = 0; + //if(ind >= 0 && (ind+0.5) < histogram_size){p = prob[int(ind+0.5)];} + float p = getProb(mat(k,j)); inl *= p; ninl *= 1.0-p; } @@ -830,24 +868,65 @@ if(!fixed_histogram_size){ nr_inliers += d; } + if(debugg_print){printf("hist = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(histogram[k]));} printf("];\n");} + if(debugg_print){printf("noise = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(noise[k]));} printf("];\n");} + if(debugg_print){printf("noisecdf = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",int(noisecdf[k]));} printf("];\n");} + if(debugg_print){printf("hist_smooth = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(blur_histogram[k]));} printf("];\n");} + if(debugg_print){printf("prob = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",prob[k]);} printf("];\n");} + if(debugg_print){printf("infront = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",infront[k]);} printf("];\n");} + if(debugg_print){printf("clf; hold on; plot(hist_smooth,'r','LineWidth',2); plot(hist,'m','LineWidth',2); plot(noise,'b','LineWidth',2); plot(prob*max(noise),'g','LineWidth',2); plot(infront*max(noise),'g','LineWidth',2);\n");} if(false){printf("meanoffset: %f stdval2: %f stdval: %f regularization: %f\n",meanval2,stdval2,noiseval,regularization);} if(!fixed_histogram_size && update_size ){ - double next_maxd = meanval2 + (stdval2 + regularization)*target_length; + + double next_maxd; + double next_mind; + if(bidir){ + next_maxd = meanval2 + 2.0*(stdval2 + regularization)*target_length; + next_mind = meanval2 - 2.0*(stdval2 + regularization)*target_length; + }else{ + next_maxd = fabs(meanval2) + (stdval2 + regularization)*target_length; + next_mind = 0; + } + double logdiff = log(next_maxd/maxd); - if(fabs(logdiff) > 0.2 && iter < 30){ + + //Ensure overlap + if(next_maxd < mind){ return;} + if(next_mind > maxd){ return;} + if(next_maxd == next_mind){ return;} + + double overlap = 0; + if(next_maxd < maxd && next_mind > mind){overlap = next_maxd-next_mind;} + if(next_maxd > maxd && next_mind < mind){overlap = maxd- mind;} + if(next_maxd < maxd && next_mind < mind){overlap = next_maxd- mind;} + if(next_maxd > maxd && next_mind > mind){overlap = maxd-next_mind;} + + double ratio = 2*overlap/(maxd+next_maxd-mind-next_mind); + double newlogdiff = log(ratio); + +// if(bidir){ +// printf("meanoffset: %f stdval2: %f stdval: %f regularization: %f\n",meanval2,stdval2,noiseval,regularization); +// printf("mind: %5.5f maxd: %5.5f noise: %5.5f noise+reg: %5.5f\n",mind,maxd,noiseval,getNoise()); +// printf("next_maxd: %5.5f maxd: %5.5f\n",next_maxd,maxd); +// printf("next_mind: %5.5f mind: %5.5f\n",next_mind,mind); +// printf("logdiff: %5.5f\n",logdiff); +// printf("newlogdiff: %5.5f\n",newlogdiff); +// } + + + //double next_maxd = meanval2 + (stdval2 + regularization)*target_length; + + + //if(fabs(logdiff) > 0.2 && iter < 30){ + if(fabs(newlogdiff) > 0.2 && iter < 30){ iter++; computeModel(mat); return; }else{iter = 0;} } - if(debugg_print){printf("hist = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(histogram[k]));} printf("];\n");} - if(debugg_print){printf("noise = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(noise[k]));} printf("];\n");} - if(debugg_print){printf("noisecdf = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",int(noisecdf[k]));} printf("];\n");} - if(debugg_print){printf("hist_smooth = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(blur_histogram[k]));} printf("];\n");} - if(debugg_print){printf("prob = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",prob[k]);} printf("];\n");} - if(debugg_print){printf("infront = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",infront[k]);} printf("];\n");} - if(debugg_print){printf("clf; hold on; plot(hist_smooth,'r'); plot(noise,'b'); plot(prob*max(noise),'g');\n");} + + //if(bidir){exit(0);} } VectorXd DistanceWeightFunction2PPR2::getProbs(MatrixXd mat){ @@ -860,22 +939,20 @@ VectorXd DistanceWeightFunction2PPR2::getProbs(MatrixXd mat){ float inl = 1; float ninl = 1; for(int k = 0; k < nr_dim; k++){ - double ind; - float p = 0; - if(bidir){ - ind = double(histogram_size)*(mat(k,j)+maxd)/(2.0*maxd); - }else{ - ind = fabs(mat(k,j))*histogram_mul; - } - - - double w2 = ind-int(ind); - double w1 = 1-w2; - - if(ind >= 0 && (ind+0.5) < histogram_size){ - if(interp){ p = prob[int(ind)]*w1 + prob[int(ind+1)]*w2; - }else{ p = prob[int(ind+0.5)];} - } +// double ind; +// float p = 0; +// if(bidir){ +// ind = double(histogram_size)*(mat(k,j)+maxd)/(2.0*maxd); +// }else{ +// ind = fabs(mat(k,j))*histogram_mul; +// } +// double w2 = ind-int(ind); +// double w1 = 1-w2; +// if(ind >= 0 && (ind+0.5) < histogram_size){ +// if(interp){ p = prob[int(ind)]*w1 + prob[int(ind+1)]*w2; +// }else{ p = prob[int(ind+0.5)];} +// } + float p = getProb(mat(k,j)); inl *= p; ninl *= 1.0-p; } @@ -1004,6 +1081,8 @@ void DistanceWeightFunction2PPR2::reset(){ nr_inliers = 9999999; regularization = startreg; maxd = startmaxd; + if(bidir){mind = -maxd;} + else{mind = 0;} histogram_size = starthistogram_size; stdval = maxd/target_length; @@ -1032,9 +1111,13 @@ double DistanceWeightFunction2PPR2::getConvergenceThreshold(){ } inline double DistanceWeightFunction2PPR2::getInd(double d, bool debugg){ +// if(bidir){ +// return 0.00001+(d+maxd)*histogram_mul2; +// }else{ +// return 0.00001+fabs(d)*histogram_mul2; +// } if(bidir){ - - return 0.00001+(d+maxd)*histogram_mul2; + return 0.00001+(d-mind)*histogram_mul2; }else{ return 0.00001+fabs(d)*histogram_mul2; } From 2b96060de86885989e7fad8f21e3716ed82dcdd6 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 20 Sep 2016 14:32:20 +0200 Subject: [PATCH 104/255] ppr3 + gaussian + ggd distribution implemented and used for normals --- quasimodo/quasimodo_models/CMakeLists.txt | 10 +- .../quasimodo_models/include/core/RGBDFrame.h | 2 +- .../weightfunctions/DistanceWeightFunction2.h | 1 + .../DistanceWeightFunction2PPR2.h | 3 + .../DistanceWeightFunction2PPR3.h | 125 +++++ .../include/weightfunctions/Distribution.h | 39 ++ .../weightfunctions/GaussianDistribution.h | 56 ++ .../GeneralizedGaussianDistribution.h | 66 +++ .../weightfunctions/SignalProcessing.h | 23 + .../quasimodo_models/src/core/RGBDFrame.cpp | 155 ++++-- .../src/modelupdater/ModelUpdater.cpp | 480 +---------------- .../DistanceWeightFunction2PPR2.cpp | 12 + .../DistanceWeightFunction2PPR3.cpp | 482 ++++++++++++++++++ .../src/weightfunctions/Distribution.cpp | 52 ++ .../weightfunctions/GaussianDistribution.cpp | 163 ++++++ .../GeneralizedGaussianDistribution.cpp | 278 ++++++++++ .../src/weightfunctions/SignalProcessing.cpp | 59 +++ 17 files changed, 1490 insertions(+), 516 deletions(-) create mode 100644 quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR3.h create mode 100644 quasimodo/quasimodo_models/include/weightfunctions/Distribution.h create mode 100644 quasimodo/quasimodo_models/include/weightfunctions/GaussianDistribution.h create mode 100644 quasimodo/quasimodo_models/include/weightfunctions/GeneralizedGaussianDistribution.h create mode 100644 quasimodo/quasimodo_models/include/weightfunctions/SignalProcessing.h create mode 100644 quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp create mode 100644 quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp create mode 100644 quasimodo/quasimodo_models/src/weightfunctions/GaussianDistribution.cpp create mode 100644 quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp create mode 100644 quasimodo/quasimodo_models/src/weightfunctions/SignalProcessing.cpp diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 114dad03..77159046 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -52,8 +52,14 @@ include_directories(/usr/local/include/) FIND_PACKAGE(Ceres REQUIRED) INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) -add_library(quasimodo_weightlib src/weightfunctions/DistanceWeightFunction2.cpp src/weightfunctions/DistanceWeightFunction2PPR.cpp src/weightfunctions/DistanceWeightFunction2PPR2.cpp) -target_link_libraries(quasimodo_weightlib ceres glog ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) +add_library(quasimodo_SignalProcessing src/weightfunctions/SignalProcessing.cpp) +target_link_libraries(quasimodo_SignalProcessing ${catkin_LIBRARIES}) + +add_library(quasimodo_distribution src/weightfunctions/Distribution.cpp src/weightfunctions/GaussianDistribution.cpp src/weightfunctions/GeneralizedGaussianDistribution.cpp) +target_link_libraries(quasimodo_distribution ceres glog ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) + +add_library(quasimodo_weightlib src/weightfunctions/DistanceWeightFunction2.cpp src/weightfunctions/DistanceWeightFunction2PPR.cpp src/weightfunctions/DistanceWeightFunction2PPR2.cpp src/weightfunctions/DistanceWeightFunction2PPR3.cpp) +target_link_libraries(quasimodo_weightlib quasimodo_SignalProcessing quasimodo_distribution ceres glog ${catkin_LIBRARIES} ${OpenCV_LIBRARIES}) add_library(quasimodo_superpoint src/core/superpoint.cpp) target_link_libraries(quasimodo_superpoint ${catkin_LIBRARIES}) diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 3c9cff2c..40a1de69 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -62,7 +62,7 @@ namespace reglib static RGBDFrame * load(Camera * cam, std::string path); std::vector getReprojections(std::vector & spvec, Eigen::Matrix4d cp, bool * maskvec, bool useDet = true); - std::vector getSuperPoints(Eigen::Matrix4d cp = Eigen::Matrix4d::Identity()); + std::vector getSuperPoints(Eigen::Matrix4d cp = Eigen::Matrix4d::Identity(), unsigned int step = 1, bool zeroinclude = true); }; } diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h index 35a65b6e..61830ce9 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h @@ -64,4 +64,5 @@ class DistanceWeightFunction2 #include "DistanceWeightFunction2PPR.h" #include "DistanceWeightFunction2PPR2.h" +#include "DistanceWeightFunction2PPR3.h" #endif // DistanceWeightFunction2_H diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h index c135a77c..2ec40118 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR2.h @@ -17,6 +17,9 @@ #include #include +#include "weightfunctions/Distribution.h" + + using namespace Eigen; namespace reglib{ diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR3.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR3.h new file mode 100644 index 00000000..38a22e01 --- /dev/null +++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR3.h @@ -0,0 +1,125 @@ +#ifndef DistanceWeightFunction2PPR3_H +#define DistanceWeightFunction2PPR3_H + +#include +#include +#include "DistanceWeightFunction2.h" + +#include "ceres/ceres.h" +#include "ceres/rotation.h" +#include "ceres/iteration_callback.h" +#include "gflags/gflags.h" +#include "glog/logging.h" + +#include +#include +#include +#include +#include + +#include "weightfunctions/Distribution.h" +#include "core/Util.h" +#include "weightfunctions/SignalProcessing.h" + + +using namespace Eigen; +namespace reglib{ + +class DistanceWeightFunction2PPR3 : public DistanceWeightFunction2 +{ +public: + + SignalProcessing * sp; + Distribution * dist; + + bool fixed_histogram_size; + + double stdval; + double stdval2; + double mulval; + double meanval; + double meanval2; + + double maxnoise; + + bool zeromean; + + double costpen; + + double maxp; + + bool first; + + bool update_size; + double target_length; + double data_per_bin; + double meanoffset; + double blur; + + double startmaxd; + double maxd; + double mind; + int histogram_size; + int starthistogram_size; + double blurval; + double stdgrow; + + double noiseval; + double startreg; + + bool ggd; + bool compute_infront; + + std::vector prob; + std::vector infront; + std::vector histogram; + std::vector blur_histogram; + std::vector noise; + std::vector noisecdf; + + int nr_refineiters; + bool refine_mean; + bool refine_mul; + bool refine_std; + + bool threshold; + bool uniform_bias; + bool scale_convergence; + double nr_inliers; + + bool rescaling; + + bool interp; + + bool max_under_mean; + + bool bidir; + int iter; + + double histogram_mul; + double histogram_mul2; + + + DistanceWeightFunction2PPR3(Distribution * dist, double maxd_ = 0.25, int histogram_size_ = 25000); + + DistanceWeightFunction2PPR3( double maxd_ = 0.25, int histogram_size_ = 25000); + ~DistanceWeightFunction2PPR3(); + + virtual void recomputeHistogram(std::vector & hist, MatrixXd & mat); + virtual void recomputeProbs(); + + virtual void computeModel(MatrixXd mat); + virtual VectorXd getProbs(MatrixXd mat); + virtual double getProb(double d, bool debugg = false); + virtual double getProbInfront(double d, bool debugg = false); + virtual double getNoise(); + virtual double getConvergenceThreshold(); + virtual bool update(); + virtual void reset(); + virtual std::string getString(); + virtual double getInd(double d, bool debugg = false); +}; + +} + +#endif // DistanceWeightFunction2PPR3test_H diff --git a/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h b/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h new file mode 100644 index 00000000..9ae533ce --- /dev/null +++ b/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h @@ -0,0 +1,39 @@ +#ifndef Distribution_H +#define Distribution_H + +#include +#include +#include +#include + +using namespace Eigen; +namespace reglib +{ + +class Distribution +{ +public: + double regularization; + double mean; + + + Distribution(); + ~Distribution(); + virtual void train(std::vector & hist, unsigned int nr_bins = 0); + virtual void update(); + + virtual double getNoise(); + virtual void setNoise(double x); + + virtual double getval(double x); + virtual double getcdf(double x); + virtual void setRegularization(double x); + virtual void print(); + virtual void getMaxdMind(double & maxd, double & mind, double prob = 0.0001); +}; + +} + +#include "GaussianDistribution.h" +#include "GeneralizedGaussianDistribution.h" +#endif // Distribution diff --git a/quasimodo/quasimodo_models/include/weightfunctions/GaussianDistribution.h b/quasimodo/quasimodo_models/include/weightfunctions/GaussianDistribution.h new file mode 100644 index 00000000..8f3614ba --- /dev/null +++ b/quasimodo/quasimodo_models/include/weightfunctions/GaussianDistribution.h @@ -0,0 +1,56 @@ +#ifndef GaussianDistribution_H +#define GaussianDistribution_H + +#include +#include +#include +#include +#include "Distribution.h" + +using namespace Eigen; +namespace reglib +{ + +class GaussianDistribution : public Distribution +{ +public: + + const double step_h = 0.00001; + const unsigned int step_iter = 25; + const double cutoff_exp = -14; + + double mul; + double stdval; + double scaledinformation; + + bool refine_mean; + bool refine_mul; + bool refine_std; + + double costpen; + bool zeromean; + int nr_refineiters; + + GaussianDistribution(bool refine_std_ = true, bool zeromean = true, bool refine_mean = false, bool refine_mul = false, double costpen_ = 3,int nr_refineiters_ = 1,double mul_ = 1, double mean_ = 0, double stdval_ = 1); + //GaussianDistribution(double mul_ = 1, double mean_ = 0, double stdval_ = 1); + ~GaussianDistribution(); + virtual void train(std::vector & hist, unsigned int nr_bins = 0); + virtual void update(); + virtual double getNoise(); + virtual void setNoise(double x); + virtual double getval(double x); + virtual double getcdf(double x); + virtual void print(); + + //virtual void getMaxdMind(double & maxd, double & mind, double prob = 0.1); + + double scoreCurrent2(double mul, double mean, double stddiv, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen); + double fitStdval2 (double mul, double mean, double std_mid, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen); + double fitMean2 (double mul, double mean, double std_mid, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen); + double fitMul2 (double mul, double mean, double std_mid, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen); + +}; + +} + +#endif // GaussianDistribution diff --git a/quasimodo/quasimodo_models/include/weightfunctions/GeneralizedGaussianDistribution.h b/quasimodo/quasimodo_models/include/weightfunctions/GeneralizedGaussianDistribution.h new file mode 100644 index 00000000..ea7d477f --- /dev/null +++ b/quasimodo/quasimodo_models/include/weightfunctions/GeneralizedGaussianDistribution.h @@ -0,0 +1,66 @@ +#ifndef GeneralizedGaussianDistribution_H +#define GeneralizedGaussianDistribution_H + +#include +#include +#include +#include +#include "Distribution.h" + +using namespace Eigen; +namespace reglib +{ + +class GeneralizedGaussianDistribution : public Distribution +{ +public: + + const double step_h = 0.00001; + const unsigned int step_iter = 25; + const double cutoff_exp = -14; + + double mul; + double stdval; + double scaledinformation; + double power; + + bool refine_mean; + bool refine_mul; + bool refine_std; + bool refine_power; + + double costpen; + bool zeromean; + int nr_refineiters; + + double start; + double stop; + std::vector numcdf_vec; + + GeneralizedGaussianDistribution(bool refine_std_ = true, bool refine_power_ = false, bool zeromean = true, bool refine_mean = false, bool refine_mul = false, double costpen_ = 3,int nr_refineiters_ = 1,double mul_ = 1, double mean_ = 0, double stdval_ = 1, double power_ = 2); + //GeneralizedGaussianDistribution(double mul_ = 1, double mean_ = 0, double stdval_ = 1); + ~GeneralizedGaussianDistribution(); + virtual void train(std::vector & hist, unsigned int nr_bins = 0); + virtual void update(); + virtual double getInp(double x); + virtual double getNoise(); + virtual void setNoise(double x); + virtual double getval(double x); + virtual double getcdf(double x); + virtual void print(); + + //virtual void getMaxdMind(double & maxd, double & mind, double prob = 0.1); + + double scoreCurrent3(double mul, double mean, double stddiv, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen); + double fitStdval3 (double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen); + double fitMean3 (double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen); + double fitMul3 (double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen); + double fitPower3 (double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen); + + virtual void update_numcdf_vec(unsigned int bins = 1000, double prob = 0.000001); + //double interp(double x); +}; + +} + +#endif // GeneralizedGaussianDistribution diff --git a/quasimodo/quasimodo_models/include/weightfunctions/SignalProcessing.h b/quasimodo/quasimodo_models/include/weightfunctions/SignalProcessing.h new file mode 100644 index 00000000..e0dacec5 --- /dev/null +++ b/quasimodo/quasimodo_models/include/weightfunctions/SignalProcessing.h @@ -0,0 +1,23 @@ +#ifndef SignalProcessing_H +#define SignalProcessing_H + +#include +#include +#include +#include + +using namespace Eigen; +namespace reglib +{ + +class SignalProcessing +{ +public: + SignalProcessing(); + ~SignalProcessing(); + virtual void process(std::vector & in, std::vector & out, double stdval, unsigned int nr_bins = 0); +}; + +} + +#endif // SignalProcessing diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 9fd2e3ed..d871c60b 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -41,6 +41,8 @@ #include + + namespace reglib { unsigned long RGBDFrame_id_counter; @@ -54,9 +56,6 @@ RGBDFrame::RGBDFrame(){ bool updated = true; void on_trackbar( int, void* ){updated = true;} -//void update(cv::Mat & err, cv::Mat & est, cv::Mat & dx, int dir){ -//} - float pred(float targetW, float targetH, int sourceW, int sourceH, int width, float * est, float * dx, float * dy){ int ind = sourceH * width + sourceW; return est[ind]+(targetW-float(sourceW))*dx[ind]+(targetH-float(sourceH))*dy[ind]; @@ -116,15 +115,16 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} stdval = sqrt(stdval/double(Xvec.size())); - DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); + DistanceWeightFunction2PPR3 * func = new DistanceWeightFunction2PPR3(); func->zeromean = true; func->maxp = 0.99; func->startreg = 0.0; - func->debugg_print = false; - //func->bidir = true; + func->debugg_print = false; func->maxd = 256.0; func->histogram_size = 256; - func->fixed_histogram_size = true; + //func->fixed_histogram_size = true; + func->fixed_histogram_size = false; + func->update_size = true; func->startmaxd = func->maxd; func->starthistogram_size = func->histogram_size; func->blurval = 0.005; @@ -294,6 +294,14 @@ RGBDFrame * RGBDFrame::clone(){ } RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals){ +// GeneralizedGaussianDistribution * ggd = new GeneralizedGaussianDistribution(); +// ggd->update_numcdf_vec(); +// printf("update_numcdf_vec done\n"); +// for(double x = -4; x <= 4; x+=0.1){ +// ggd->getcdf(x); +// } +// printf("cdf loop done\n"); +// exit(0); keyval = ""; sweepid = -1; @@ -366,42 +374,79 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt std::vector Xvec; - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; - - if(z2 > 0 || z > 0){ - dedata[3*ind+1] = fabs((z-z2)/(z*z+z2*z2)); - Xvec.push_back(dedata[3*ind+1]); - } - } - - if(h > 1){ - int dir = -width; - int other2 = ind+2*dir; - int other = ind+dir; +// for(unsigned int w = 1; w < width-1;w++){ +// for(unsigned int h = 1; h < height-1;h++){ +// int ind = h*width+w; +// float z = idepth*float(depthdata[ind]); + +// if(w > 1){ +// int dir = -1; +// int other2 = ind+2*dir; +// int other = ind+dir; + +// float z3 = idepth*float(depthdata[other2]); +// float z2 = idepth*float(depthdata[other]); +// z2 = 2*z2-z3; + +// double noise1 = z*z; +// double noise2 = z2*z2; +// double noise3 = sqrt(noise1*noise1+noise2*noise2); + +// if(z2 > 0 || z > 0){ +// dedata[3*ind+1] = fabs((z-z2)/noise3);//(z*z+z2*z2)); +// Xvec.push_back(dedata[3*ind+1]); +// } +// } + +// if(h > 1){ +// int dir = -width; +// int other2 = ind+2*dir; +// int other = ind+dir; + +// float z3 = idepth*float(depthdata[other2]); +// float z2 = idepth*float(depthdata[other]); +// z2 = 2*z2-z3; + +// double noise1 = z*z; +// double noise2 = z2*z2; +// double noise3 = sqrt(noise1*noise1+noise2*noise2); +// if(z2 > 0 || z > 0){ +// dedata[3*ind+2] = fabs((z-z2)/noise3);//(z*z+z2*z2)); +// Xvec.push_back(dedata[3*ind+2]); +// } +// } +// } +// } - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; + int step = 1; + for(unsigned int w = step; w < width-step;w++){ + for(unsigned int h = step; h < height-step;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + float z2 = 0.5*idepth*float(depthdata[ind-step]+depthdata[ind+step]); + double noise1 = z*z; + double noise2 = z2*z2; + double noise3 = sqrt(noise1*noise1+noise2*noise2); + if(z2 > 0 || z > 0){ + dedata[3*ind+1] = fabs((z-z2)/noise3); + Xvec.push_back(dedata[3*ind+1]); + } + } - if(z2 > 0 || z > 0){ - dedata[3*ind+2] = fabs((z-z2)/(z*z+z2*z2)); - Xvec.push_back(dedata[3*ind+2]); - } - } - } - } + if(h > 1){ + float z2 = 0.5*idepth*float(depthdata[ind-step*width]+depthdata[ind+step*width]); + double noise1 = z*z; + double noise2 = z2*z2; + double noise3 = sqrt(noise1*noise1+noise2*noise2); + if(z2 > 0 || z > 0){ + dedata[3*ind+2] = fabs((z-z2)/noise3); + Xvec.push_back(dedata[3*ind+2]); + } + } + } + } @@ -412,10 +457,10 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} stdval = sqrt(stdval/double(Xvec.size())); - DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); + DistanceWeightFunction2PPR3 * funcZ = new DistanceWeightFunction2PPR3(); funcZ->zeromean = true; - funcZ->startreg = 0.002; - funcZ->debugg_print = false; + funcZ->startreg = 0.001; + funcZ->debugg_print = true; funcZ->bidir = false; funcZ->maxp = 0.999999; funcZ->maxd = 0.1; @@ -444,6 +489,9 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt delete funcZ; + + + cv::Mat det; det.create(height,width,CV_8UC1); unsigned char * detdata = (unsigned char*)det.data; @@ -452,6 +500,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } +// cv::namedWindow( "det", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det); +// cv::waitKey(0); int dilation_size = 4; @@ -2313,7 +2363,7 @@ std::vector RGBDFrame::getReprojections(std::vector RGBDFrame::getSuperPoints(Eigen::Matrix4d cp){ +std::vector RGBDFrame::getSuperPoints(Eigen::Matrix4d cp, unsigned int step, bool zeroinclude){ unsigned char * rgbdata = (unsigned char *)(rgb.data); unsigned short * depthdata = (unsigned short *)(depth.data); float * normalsdata = (float *)(normals.data); @@ -2332,8 +2382,10 @@ std::vector RGBDFrame::getSuperPoints(Eigen::Matrix4d cp){ std::vector ret; ret.resize(width*height); - for(unsigned long h = 0; h < height;h++){ - for(unsigned long w = 0; w < width;w++){ + unsigned long count = 0; + + for(unsigned long h = 0; h < height;h += step){ + for(unsigned long w = 0; w < width;w += step){ unsigned int ind = h * width + w; //superpoint & p = ret[ind]; Eigen::Vector3f rgb (rgbdata[3*ind+0],rgbdata[3*ind+1],rgbdata[3*ind+2]); @@ -2353,12 +2405,17 @@ std::vector RGBDFrame::getSuperPoints(Eigen::Matrix4d cp){ float tny = m10*nx + m11*ny + m12*nz; float tnz = m20*nx + m21*ny + m22*nz; - ret[ind] = superpoint(Vector3f(tx,ty,tz),Vector3f(tnx,tny,tnz),rgb, getInformation(z), 1, 0); + //ret[ind] = superpoint(Vector3f(tx,ty,tz),Vector3f(tnx,tny,tnz),rgb, getInformation(z), 1, 0); + ret[count++] = superpoint(Vector3f(tx,ty,tz),Vector3f(tnx,tny,tnz),rgb, getInformation(z), 1, 0); }else{ - ret[ind] = superpoint(Eigen::Vector3f(0,0,0),Eigen::Vector3f(0,0,0),rgb, 0, 1, 0); + //ret[ind] = superpoint(Eigen::Vector3f(0,0,0),Eigen::Vector3f(0,0,0),rgb, 0, 1, 0); + if(zeroinclude){ + ret[count++] = superpoint(Eigen::Vector3f(0,0,0),Eigen::Vector3f(0,0,0),rgb, 0, 1, 0); + } } } } + ret.resize(count); return ret; } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 9dc4330f..a4c71e30 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1937,18 +1937,22 @@ double normalCFD(double value) } void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ - // Camera * camera = frame1->camera; - // const unsigned int width = camera->width; - // const unsigned int height = camera->height; - // unsigned long nr_pixels = width*height; - unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); + std::vector framesp1_test = frame1->getSuperPoints(p,10,false); + std::vector rr_vec_test = frame2->getReprojections(framesp1_test,Eigen::Matrix4d::Identity(),0,false); + + double inlierratio = double(rr_vec_test.size())/double(framesp1_test.size()); + //printf("inlierratio: %f nr_points: %i nr rep: %i\n",inlierratio,framesp1_test.size(),rr_vec_test.size()); + if( inlierratio < 0.01 ){return ;} + std::vector framesp1 = frame1->getSuperPoints(p); std::vector framesp2 = frame2->getSuperPoints();//p); std::vector rr_vec = frame2->getReprojections(framesp1,Eigen::Matrix4d::Identity(),0,false); + inlierratio = double(rr_vec.size())/double(framesp1.size()); + //printf("inlierratio: %f nr_points: %i nr rep: %i\n",inlierratio,framesp1.size(),rr_vec.size()); unsigned long nr_rr = rr_vec.size(); printf("getDynamicWeights nr_rr: %i\n",nr_rr); @@ -2013,460 +2017,6 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); viewer->spin(); } - - // std::vector residualsZ; - // double stdval = 0; - // residualsZ.push_back(d); - // stdval += residualsZ.back()*residualsZ.back(); - // stdval = sqrt(stdval/double(nr_rr)); - - // DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); - // func->zeromean = true; - // func->maxp = 0.99; - // func->startreg = 0.0001; - // func->debugg_print = true; - // func->maxd = 0.1; - // func->startmaxd = func->maxd; - // func->histogram_size = 100; - // func->starthistogram_size = func->histogram_size; - // func->stdval2 = stdval; - // func->maxnoise = stdval; - // func->reset(); - // ((DistanceWeightFunction2*)func)->computeModel(residualsZ); - - - // double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - - // d = fabs(d); - // if(tz > dst_z){d = -d;} - // if(store_distance){ - // dvec.push_back(d); - // nvec.push_back(1-surface_angle); - // }else{ - - // double p_overlap_angle = nfunc->getProb(1-surface_angle); - // double p_overlap = dfunc->getProb(d); - // double p_occlusion = dfunc->getProbInfront(d); - - // p_overlap *= p_overlap_angle; - - // if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0){ - // interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); - // interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap)); - // } - - // if(debugg){ - // src_cloud->points[src_ind].r = 255.0 * p_overlap; - // src_cloud->points[src_ind].g = 255.0 * p_occlusion; - // src_cloud->points[src_ind].b = 0; - // } - - // if(dst_detdata[dst_ind] != 0){continue;} - // if(src_detdata[src_ind] != 0){continue;} - - // overlaps[src_ind] = std::min(0.9,std::max(overlaps[src_ind],p_overlap)); - // occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); - // } - // } - //} - - // for(unsigned long ind = 0; ind < nr_rr;ind++){ - // double p = func->getProb(residualsZ[ind]); - // if(p > 0.5){sumw[rr_vec[ind].dst_ind] += p;} - // } - - // for(unsigned long ind = 0; ind < nr_rr;ind++){ - // double p = func->getProb(residualsZ[ind]); - // if(p > 0.5){ - // ReprojectionResult & rr = rr_vec[ind]; - // superpoint & src_p = spvec[rr.src_ind]; - // superpoint & dst_p = framesp[rr.dst_ind]; - // float weight = p/sumw[rr.dst_ind]; - // src_p.merge(dst_p,weight); - // } - // } - //delete func; - - // for(unsigned long ind = 0; ind < nr_pixels;ind++){ - // if(maskvec[ind] != 0 && sumw[ind] < 0.5){ - // superpoint & sp = framesp[ind]; - // if(sp.point_information > 0){ - // spvec.push_back(sp); - // } - // } - // } - - - /* - - - unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); - float * src_normalsdata = (float *)(frame1->normals.data); - - - unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); - unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); - - Camera * src_camera = frame1->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - - unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); - float * dst_normalsdata = (float *)(frame2->normals.data); - - Camera * dst_camera = frame2->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - //bool debugg = true; - pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); - if(debugg){ - src_cloud->points.resize(src_width*src_height); - for(unsigned int src_w = 0; src_w < src_width;src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; - - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; - point.r = 0; - point.g = 0; - point.b = 0; - src_cloud->points[src_ind] = point; - } - } - } - - dst_cloud->points.resize(dst_width*dst_height); - for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - int dst_ind = dst_h*dst_width+dst_w; - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - pcl::PointXYZRGBNormal point; - point.x = dst_x; - point.y = dst_y; - point.z = dst_z; - point.r = 255; - point.g = 0; - point.b = 255; - dst_cloud->points[dst_ind] = point; - } - } - } - } - - if(debugg){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); - viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); - viewer->spin(); - } - - - exit(0); - */ - /* - if(nr_rr > 10){ - std::vector residualsZ; - double stdval = 0; - for(unsigned long ind = 0; ind < nr_rr;ind++){ - ReprojectionResult & rr = rr_vec[ind]; - superpoint & src_p = spvec[rr.src_ind]; - superpoint & dst_p = framesp[rr.dst_ind]; - double src_variance = 1.0/src_p.point_information; - double dst_variance = 1.0/dst_p.point_information; - double total_variance = src_variance+dst_variance; - double total_stdiv = sqrt(total_variance); - double d = rr.residualZ; - residualsZ.push_back(d/total_stdiv); - stdval += residualsZ.back()*residualsZ.back(); - } - stdval = sqrt(stdval/double(nr_rr)); - - DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); - func->zeromean = true; - func->maxp = 0.99; - func->startreg = 0.001; - func->debugg_print = false; - func->maxd = 0.1; - func->startmaxd = func->maxd; - func->histogram_size = 100; - func->starthistogram_size = func->histogram_size; - func->stdval2 = stdval; - func->maxnoise = stdval; - func->reset(); - ((DistanceWeightFunction2*)func)->computeModel(residualsZ); - - for(unsigned long ind = 0; ind < nr_rr;ind++){ - double p = func->getProb(residualsZ[ind]); - if(p > 0.5){sumw[rr_vec[ind].dst_ind] += p;} - } - - for(unsigned long ind = 0; ind < nr_rr;ind++){ - double p = func->getProb(residualsZ[ind]); - if(p > 0.5){ - ReprojectionResult & rr = rr_vec[ind]; - superpoint & src_p = spvec[rr.src_ind]; - superpoint & dst_p = framesp[rr.dst_ind]; - float weight = p/sumw[rr.dst_ind]; - src_p.merge(dst_p,weight); - } - } - delete func; - } - - for(unsigned long ind = 0; ind < nr_pixels;ind++){ - if(maskvec[ind] != 0 && sumw[ind] < 0.5){spvec.push_back(framesp[ind]);} - } - - if(viewer != 0){ - pcl::PointCloud::Ptr cloud = getPointCloudFromVector(spvec,1); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "model"); - viewer->spin(); - viewer->removeAllPointClouds(); - } -*/ - - - /* - unsigned char * src_rgbdata = (unsigned char *)(frame1->rgb.data); - unsigned short * src_depthdata = (unsigned short *)(frame1->depth.data); - float * src_normalsdata = (float *)(frame1->normals.data); - - - unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); - unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); - - Camera * src_camera = frame1->camera; - const unsigned int src_width = src_camera->width; - const unsigned int src_height = src_camera->height; - const float src_idepth = src_camera->idepth_scale; - const float src_cx = src_camera->cx; - const float src_cy = src_camera->cy; - const float src_ifx = 1.0/src_camera->fx; - const float src_ify = 1.0/src_camera->fy; - - unsigned char * dst_rgbdata = (unsigned char *)(frame2->rgb.data); - unsigned short * dst_depthdata = (unsigned short *)(frame2->depth.data); - float * dst_normalsdata = (float *)(frame2->normals.data); - - Camera * dst_camera = frame2->camera; - const unsigned int dst_width = dst_camera->width; - const unsigned int dst_height = dst_camera->height; - const float dst_idepth = dst_camera->idepth_scale; - const float dst_cx = dst_camera->cx; - const float dst_cy = dst_camera->cy; - const float dst_ifx = 1.0/dst_camera->fx; - const float dst_ify = 1.0/dst_camera->fy; - const float dst_fx = dst_camera->fx; - const float dst_fy = dst_camera->fy; - - const unsigned int dst_width2 = dst_camera->width - 2; - const unsigned int dst_height2 = dst_camera->height - 2; - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - //bool debugg = true; - pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); - if(debugg){ - - src_cloud->points.resize(src_width*src_height); - for(unsigned int src_w = 0; src_w < src_width;src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; - - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - - pcl::PointXYZRGBNormal point; - point.x = tx; - point.y = ty; - point.z = tz; - point.r = 0; - point.g = 0; - point.b = 0; - src_cloud->points[src_ind] = point; - } - } - } - - dst_cloud->points.resize(dst_width*dst_height); - for(unsigned int dst_w = 0; dst_w < dst_width;dst_w++){ - for(unsigned int dst_h = 0; dst_h < dst_height;dst_h++){ - int dst_ind = dst_h*dst_width+dst_w; - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - pcl::PointXYZRGBNormal point; - point.x = dst_x; - point.y = dst_y; - point.z = dst_z; - point.r = 255; - point.g = 0; - point.b = 255; - dst_cloud->points[dst_ind] = point; - } - } - } - } - - int testsum = 0; - - - for(unsigned int src_w = 0; src_w < src_width;src_w++){ - for(unsigned int src_h = 0; src_h < src_height;src_h++){ - int src_ind = src_h*src_width+src_w; - - - float src_z = src_idepth*float(src_depthdata[src_ind]); - float src_nx = src_normalsdata[3*src_ind+0]; - if(src_z > 0 && src_nx != 2){ - float src_ny = src_normalsdata[3*src_ind+1]; - float src_nz = src_normalsdata[3*src_ind+2]; - - float src_x = (float(src_w) - src_cx) * src_z * src_ifx; - float src_y = (float(src_h) - src_cy) * src_z * src_ify; - - - float tz = m20*src_x + m21*src_y + m22*src_z + m23; - if(tz <= 0){continue;} - - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float itz = 1.0/tz; - float dst_w = dst_fx*tx*itz + dst_cx; - float dst_h = dst_fy*ty*itz + dst_cy; - - if((dst_w > 0) && (dst_h > 0) && (dst_w < dst_width2) && (dst_h < dst_height2)){ - unsigned int dst_ind = unsigned(dst_h+0.5) * dst_width + unsigned(dst_w+0.5); - - float dst_z = dst_idepth*float(dst_depthdata[dst_ind]); - float dst_nx = dst_normalsdata[3*dst_ind+0]; - if(dst_z > 0 && dst_nx != 2){ - float dst_ny = dst_normalsdata[3*dst_ind+1]; - float dst_nz = dst_normalsdata[3*dst_ind+2]; - - float dst_x = (float(dst_w) - dst_cx) * dst_z * dst_ifx; - float dst_y = (float(dst_h) - dst_cy) * dst_z * dst_ify; - - double dst_noise = dst_z * dst_z; - double point_noise = src_z * src_z; - - float tnx = m00*src_nx + m01*src_ny + m02*src_nz; - float tny = m10*src_nx + m11*src_ny + m12*src_nz; - float tnz = m20*src_nx + m21*src_ny + m22*src_nz; - - double d = (tnx*(dst_x-tx) + tny*(dst_y-ty) + tnz*(dst_z-tz)) / sqrt(dst_noise*dst_noise + point_noise*point_noise); - - double surface_angle = tnx*dst_nx+tny*dst_ny+tnz*dst_nz; - - d = fabs(d); - if(tz > dst_z){d = -d;} - if(store_distance){ - dvec.push_back(d); - nvec.push_back(1-surface_angle); - }else{ - - double p_overlap_angle = nfunc->getProb(1-surface_angle); - double p_overlap = dfunc->getProb(d); - double p_occlusion = dfunc->getProbInfront(d); - - p_overlap *= p_overlap_angle; - - if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0){ - interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); - interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap)); - } - - if(debugg){ - src_cloud->points[src_ind].r = 255.0 * p_overlap; - src_cloud->points[src_ind].g = 255.0 * p_occlusion; - src_cloud->points[src_ind].b = 0; - } - - if(dst_detdata[dst_ind] != 0){continue;} - if(src_detdata[src_ind] != 0){continue;} - - overlaps[src_ind] = std::min(0.9,std::max(overlaps[src_ind],p_overlap)); - occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); - } - } - } - } - } - } - - if(debugg){ - printf("testsum: %i\n",testsum); - viewer->removeAllPointClouds(); - viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); - viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); - viewer->spin(); - } -*/ } void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ @@ -2520,7 +2070,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dstdval = sqrt(dstdval/double(dvec.size()-1)); - DistanceWeightFunction2PPR2 * dfuncTMP = new DistanceWeightFunction2PPR2(); + DistanceWeightFunction2PPR2 * dfuncTMP = new DistanceWeightFunction2PPR2(); dfunc = dfuncTMP; dfuncTMP->refine_mean = true; dfuncTMP->zeromean = false; @@ -2544,13 +2094,15 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dfunc->computeModel(dvec); - DistanceWeightFunction2PPR2 * nfuncTMP = new DistanceWeightFunction2PPR2(); + GeneralizedGaussianDistribution * ggdnfunc = new GeneralizedGaussianDistribution(true,true); + ggdnfunc->nr_refineiters = 4; + DistanceWeightFunction2PPR3 * nfuncTMP = new DistanceWeightFunction2PPR3(ggdnfunc); nfunc = nfuncTMP; - nfuncTMP->startreg = 0.01; - nfuncTMP->debugg_print = false; + nfuncTMP->startreg = 0.00; + nfuncTMP->debugg_print = true; nfuncTMP->bidir = false; nfuncTMP->zeromean = true; - nfuncTMP->maxp = 0.999999; + nfuncTMP->maxp = 0.999; nfuncTMP->maxd = 1.0; nfuncTMP->histogram_size = 1000; nfuncTMP->fixed_histogram_size = true; diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp index f485c97c..e309d404 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp @@ -119,6 +119,11 @@ class Gaussian3 { { return 0.5 * erfc(-((x-mean)/stdval) * M_SQRT1_2); } + + + void print(){ + printf("Gaussian3:: mul = %5.5f mean = %5.5f stdval = %5.5f\n",mul,mean,stdval); + } }; class gaussian2 { @@ -803,6 +808,13 @@ void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ Gaussian3 g = getModel(stdval, blur_histogram, uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); +// g.print(); + +// GaussianDistribution * gd = new GaussianDistribution(refine_std,zeromean,refine_mean,refine_mul,costpen,nr_refineiters); +// gd->train(blur_histogram,histogram_size); +// gd->print(); +// delete gd; + g.stdval = std::max(0.0001*double(histogram_size)/(maxd),std::min(g.stdval,maxnoise*double(histogram_size)/maxd)); //noiseval = maxd*g.stdval/float(histogram_size); diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp new file mode 100644 index 00000000..df0adb38 --- /dev/null +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp @@ -0,0 +1,482 @@ +#include "weightfunctions/DistanceWeightFunction2PPR3.h" + +#include +#include +#include +#include +#include + +double getTime3(){ + struct timeval start1; + gettimeofday(&start1, NULL); + return double(start1.tv_sec+(start1.tv_usec/1000000.0)); +} + +namespace reglib{ + +DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3(Distribution * dist_, double maxd_, int histogram_size_){ + + fixed_histogram_size = false; + + regularization = 0.1; + maxd = maxd_; + mind = 0; + histogram_size = histogram_size_; + starthistogram_size = histogram_size; + stdval = blurval; + + maxnoise = 99999999999999; + noiseval = 100.0; + + prob.resize(histogram_size+1); + prob[histogram_size] = 0; + + infront.resize(histogram_size+1); + infront[histogram_size] = 1; + + histogram.resize(histogram_size+1); + blur_histogram.resize(histogram_size+1); + noise.resize(histogram_size+1); + noisecdf.resize(histogram_size+1); + + mulval = 1; + meanval = 0; + + startmaxd = maxd_; + + + scale_convergence = true; + nr_inliers = 1; + + target_length = 10.0; + + bidir = false; + iter = 0; + + maxp = 0.99; + + first = true; + + + startreg = 2; + blurval = 4; + stdval = 100; + noiseval = 100.0; + debugg_print = true; + threshold = false; + blur = 0.03; + data_per_bin = 30; + + + update_size = true;//(setting&2 != 1); + max_under_mean = true;//(setting&1 != 0); + interp = true;//(setting&1 != 0); + + nr_refineiters = 1; + convergence_threshold = 0.05; + compute_infront = false; + + sp = new SignalProcessing(); + dist = dist_;//new GeneralizedGaussianDistribution(true,true);//GaussianDistribution();//GeneralizedGaussianDistribution();//GaussianDistribution(); +} + +DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3( double maxd_, int histogram_size_){ + + fixed_histogram_size = false; + + regularization = 0.1; + maxd = maxd_; + mind = 0; + histogram_size = histogram_size_; + starthistogram_size = histogram_size; + stdval = blurval; + + maxnoise = 99999999999999; + noiseval = 100.0; + + prob.resize(histogram_size+1); + prob[histogram_size] = 0; + + infront.resize(histogram_size+1); + infront[histogram_size] = 1; + + histogram.resize(histogram_size+1); + blur_histogram.resize(histogram_size+1); + noise.resize(histogram_size+1); + noisecdf.resize(histogram_size+1); + + mulval = 1; + meanval = 0; + + startmaxd = maxd_; + + + scale_convergence = true; + nr_inliers = 1; + + target_length = 10.0; + + bidir = false; + iter = 0; + + maxp = 0.99; + + first = true; + + + startreg = 2; + blurval = 4; + stdval = 100; + noiseval = 100.0; + debugg_print = true; + threshold = false; + blur = 0.03; + data_per_bin = 30; + + + update_size = true;//(setting&2 != 1); + max_under_mean = true;//(setting&1 != 0); + interp = true;//(setting&1 != 0); + + nr_refineiters = 1; + convergence_threshold = 0.05; + compute_infront = false; + + sp = new SignalProcessing(); + dist = new GeneralizedGaussianDistribution(true,true);//GaussianDistribution();//GeneralizedGaussianDistribution();//GaussianDistribution(); +} + +DistanceWeightFunction2PPR3::~DistanceWeightFunction2PPR3(){ + if(sp != 0){delete sp;} + if(dist != 0){delete dist;} +} + +double DistanceWeightFunction2PPR3::getNoise(){return regularization+noiseval;} + +void DistanceWeightFunction2PPR3::recomputeHistogram(std::vector & hist, MatrixXd & mat){ + const unsigned int nr_data = mat.cols(); + const int nr_dim = mat.rows(); + histogram_mul2 = double(histogram_size)/(maxd-mind); + for(int j = 0; j < histogram_size; j++){hist[j] = 0;} + for(unsigned int j = 0; j < nr_data; j++){ + for(int k = 0; k < nr_dim; k++){ + double ind = getInd(mat(k,j)); + if(ind >= 0 && (ind+0.00001) < histogram_size){ + hist[int(ind+0.00001)]++; + } + } + } +} + +void DistanceWeightFunction2PPR3::recomputeProbs(){ + for(int k = 0; k < histogram_size; k++){noise[k] = dist->getval(k);} + if(compute_infront){ + for(int k = 0; k < histogram_size; k++){noisecdf[k] = dist->getcdf(k);} + } + + double maxhist = blur_histogram[0]; + for(int k = 1; k < histogram_size; k++){maxhist = std::max(maxhist,double(blur_histogram[k]));} + double minhist = maxhist*0.01; + + + for(int k = 0; k < histogram_size; k++){ + if(max_under_mean && k < dist->mean){ prob[k] = maxp; } + else{ + double hs = std::max(minhist,std::max(1.0,double(blur_histogram[k]))); + prob[k] = std::min(maxp , noise[k]/hs);//never fully trust any data + } + infront[k] = (1-prob[k])*noisecdf[k]; + } +} + +void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){ + const unsigned int nr_data = mat.cols(); + const int nr_dim = mat.rows(); + double start_time; + + if(debugg_print){printf("\n################################################### ITER:%i ############################################################\n",iter);} + if(!fixed_histogram_size){ + if(first){ + first = false; + double sum = 0; + for(unsigned int j = 0; j < nr_data; j++){ + for(int k = 0; k < nr_dim; k++){sum += mat(k,j)*mat(k,j);} + } + double stdval = sqrt(sum / double(nr_data*nr_dim)); + if(debugg_print){printf("stdval: %f\n",stdval);} + dist->setNoise(stdval); + } + + if(update_size){ + double next_maxd; + double next_mind; + start_time = getTime(); + dist->getMaxdMind(next_maxd,next_mind,0.05); + next_maxd *= (maxd-mind)/double(histogram_size); + next_mind *= (maxd-mind)/double(histogram_size); + if(debugg_print){printf("getMaxdMind time: %5.5fs\n",getTime()-start_time);} + if(!bidir){next_mind = 0;} + mind = next_mind; + maxd = next_maxd; + + start_time = getTime(); + double nr_inside = 0; + for(unsigned int j = 0; j < nr_data; j++){ + for(int k = 0; k < nr_dim; k++){ + double d = mat(k,j); + if(mind < d && d < maxd){nr_inside++;} + } + } + if(debugg_print){printf("nr_inside time: %5.5fs\n",getTime()-start_time);} + + int new_histogram_size = std::min(int(histogram.size()),std::max(50,int(0.5 + nr_inside/data_per_bin))); + histogram_size = std::min(1000,new_histogram_size); + blurval = blur*double(histogram_size)*float(histogram_size)/float(new_histogram_size); + if(debugg_print){printf("nr_inside: %f histogram_size: %i blurval: %f\n",nr_inside,histogram_size,blurval);} + } + } + + if(debugg_print){printf("histogram_size: %5.5f maxd: %5.5f mind: %5.5f\n",double(histogram_size),maxd,mind);} + + start_time = getTime(); + recomputeHistogram(histogram,mat); + if(debugg_print){printf("computing histogram time: %5.5fs\n",getTime()-start_time);} + + + start_time = getTime(); + sp->process(histogram,blur_histogram,blurval,histogram_size); + if(debugg_print){printf("SignalProcessing time: %5.5fs\n",getTime()-start_time);} + + + start_time = getTime(); + dist->setRegularization(double(histogram_size)*regularization/(maxd-mind)); + dist->train(blur_histogram,histogram_size); + dist->update(); + if(debugg_print){printf("train time: %5.5fs\n",getTime()-start_time);dist->print();} + + start_time = getTime(); + recomputeProbs(); + if(debugg_print){printf("recomputeProbs time: %5.5fs\n",getTime()-start_time);} + + start_time = getTime(); + getProbs(mat); + if(debugg_print){printf("get number of inliers time: %5.5fs nr_inliers: %f\n",getTime()-start_time,nr_inliers);} + + if(debugg_print){printf("hist = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(histogram[k]));} printf("];\n");} + if(debugg_print){printf("hist_smooth = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(blur_histogram[k]));} printf("];\n");} + if(debugg_print){printf("noise = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(noise[k]));} printf("];\n");} + if(debugg_print){printf("noisecdf = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",int(noisecdf[k]));} printf("];\n");} + if(debugg_print){printf("prob = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",prob[k]);} printf("];\n");} + if(debugg_print){printf("infront = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",infront[k]);} printf("];\n");} + if(debugg_print){printf("clf; hold on; plot(hist_smooth,'r','LineWidth',2); plot(hist,'m','LineWidth',2); plot(noise,'b','LineWidth',2); plot(prob*max(noise),'g','LineWidth',2); plot(infront*max(noise),'g','LineWidth',2);\n");} + +// if(false){printf("meanoffset: %f stdval2: %f stdval: %f regularization: %f\n",meanval2,stdval2,noiseval,regularization);} + if(!fixed_histogram_size && update_size ){ + + double next_maxd; + double next_mind; + start_time = getTime(); + + dist->getMaxdMind(next_maxd,next_mind,0.05); + next_maxd *= (maxd-mind)/double(histogram_size); + next_mind *= (maxd-mind)/double(histogram_size); + if(debugg_print){ + printf("getMaxdMind time: %5.5fs\n",getTime()-start_time); + printf("current: %f %f\n",mind,maxd); + printf("next: %f %f\n",next_mind,next_maxd); + } + + if(!bidir){next_mind = 0;} + + //Ensure overlap + if(next_maxd < mind){ return;} + if(next_mind > maxd){ return;} + if(next_maxd == next_mind){ return;} + + if(debugg_print){printf("next: %f %f\n",next_mind,next_maxd);} + double overlap = 0; + if(next_maxd <= maxd && next_mind >= mind){ overlap = next_maxd-next_mind;} + else if(next_maxd >= maxd && next_mind <= mind){overlap = maxd- mind;} + else if(next_maxd <= maxd && next_mind <= mind){overlap = next_maxd- mind;} + else if(next_maxd >= maxd && next_mind >= mind){overlap = maxd-next_mind;} + + double ratio = 2*overlap/(maxd+next_maxd-mind-next_mind); + double newlogdiff = log(ratio); + if(debugg_print){printf("overlap: %5.5f ratio: %5.5f newlogdiff: %5.5f\n",overlap,ratio,newlogdiff);} + + if(fabs(newlogdiff) > 0.2 && iter < 3){ + iter++; + computeModel(mat); + return; + }else{iter = 0;} + } +} + +VectorXd DistanceWeightFunction2PPR3::getProbs(MatrixXd mat){ + const unsigned int nr_data = mat.cols(); + const int nr_dim = mat.rows(); + + nr_inliers = 0; + VectorXd weights = VectorXd(nr_data); + for(unsigned int j = 0; j < nr_data; j++){ + float inl = 1; + float ninl = 1; + for(int k = 0; k < nr_dim; k++){ + float p = getProb(mat(k,j)); + inl *= p; + ninl *= 1.0-p; + } + double d = inl / (inl+ninl); + nr_inliers += d; + weights(j) = d; + } + + if(threshold){ + for(unsigned int j = 0; j < nr_data; j++){weights(j) = weights(j) > 0.5;} + } + return weights; +} + +double DistanceWeightFunction2PPR3::getProb(double d, bool debugg){ + double ind = getInd(d,debugg); + float p = 0; + if(interp){ + double w2 = ind-int(ind); + double w1 = 1-w2; + if(ind >= 0 && (ind+1) < histogram_size){ + p = prob[int(ind)]*w1 + prob[int(ind+1)]*w2; + } + }else{ + if(ind >= 0 && ind < histogram_size){ + p = prob[int(ind)]; + } + } + + + if(debugg){printf("d: %5.5f -> ind: %5.5f -> p: %5.5f histogramsize: %5.5i maxd: %5.5f\n",d,ind,p,histogram_size,maxd);} + return p; +} + + +double DistanceWeightFunction2PPR3::getProbInfront(double d, bool debugg){ + double ind = getInd(d,debugg); + float p; + if(ind > meanval){ p = 1;} + else{ p = 0;} + + if(interp){ + double w2 = ind-int(ind); + double w1 = 1-w2; + if(ind >= 0 && (ind+1) < histogram_size){ + p = infront[int(ind)]*w1 + infront[int(ind+1)]*w2; + } + }else{ + if(ind >= 0 && ind < histogram_size){ + p = infront[int(ind)]; + } + } + + if(debugg){printf("d: %5.5f -> ind: %5.5f -> infront: %5.5f histogramsize: %5.5i maxd: %5.5f\n",d,ind,p,histogram_size,maxd);} + return p; +} + +bool DistanceWeightFunction2PPR3::update(){ +// if(true){ + std::vector new_histogram = histogram; + std::vector new_blur_histogram = blur_histogram; + + float old_sum_prob = 0; + for(int k = 0; k < histogram_size; k++){old_sum_prob += prob[k] * histogram[k];} + +// Gaussian3 g = Gaussian3(mulval,meanval,stdval);//getModel(stdval,blur_histogram,uniform_bias); + + int iteration = 0; + double regularization_before = regularization; + for(int i = 0; i < 500; i++){ + iteration++; + regularization *= 0.5; + double before_noise = dist->getNoise(); + dist->setRegularization(regularization); + double after_noise = dist->getNoise(); + + if(0.99*before_noise < after_noise){return true;} + + float new_sum_prob = 0; + for(int k = 0; k < histogram_size; k++){ + double hs = new_blur_histogram[k] +0.0000001; + new_sum_prob += std::min(maxp , dist->getval(k)/hs) * new_histogram[k]; + } + + + //printf("new_sum_prob %f old_sum_prob %f ratio: %f\n",new_sum_prob,old_sum_prob,new_sum_prob/old_sum_prob); + if(new_sum_prob < 0.99*old_sum_prob || regularization*10 < regularization_before){return false;} + + //recomputeProbs(); + + + //double change = histogram_size*regularization/(maxd-mind); +// //printf("DistanceWeightFunction2PPR3::update() %i -> reg %f change %f\n",i,regularization,change); + + //if(change < 0.01*dist->getNoise()){return true;} + +// g.stdval += change; +// g.update(); + +// float new_sum_prob = 0; +// for(int k = 0; k < histogram_size; k++){ +// double hs = new_blur_histogram[k] +0.0000001; +// new_sum_prob += std::min(maxp , g.getval(k)/hs) * new_histogram[k]; +// } + +// //printf("new_sum_prob %f old_sum_prob %f ratio: %f\n",new_sum_prob,old_sum_prob,new_sum_prob/old_sum_prob); +// if(new_sum_prob < 0.99*old_sum_prob || regularization < regularization_before/double(target_length*5)){return false;} +// g.stdval -= change; +// g.update(); + } + return true; +// }else{ +// regularization *= 0.5; +// } +} + +void DistanceWeightFunction2PPR3::reset(){ + nr_inliers = 9999999; + regularization = startreg; + maxd = startmaxd; + if(bidir){mind = -maxd;} + else{mind = 0;} + histogram_size = starthistogram_size; + + stdval = maxd/target_length; + stdval2 = maxd/target_length; + meanval = 0; + meanval2 = 0; + iter = 0; + first = true; + +} + +std::string DistanceWeightFunction2PPR3::getString(){ + char buf [1024]; + sprintf(buf,"PPR_%i_%i_%i_%i",int(1000.0*startreg),int(1000.0*blur),interp,int(data_per_bin)); + return std::string(buf); +} + +double DistanceWeightFunction2PPR3::getConvergenceThreshold(){ + if(scale_convergence){ + return convergence_threshold*getNoise()/sqrt(nr_inliers); + }else{ + return convergence_threshold; + } +} + +inline double DistanceWeightFunction2PPR3::getInd(double d, bool debugg){ + if(bidir){ + return 0.00001+(d-mind)*histogram_mul2; + }else{ + return 0.00001+fabs(d)*histogram_mul2; + } +} +} + + diff --git a/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp new file mode 100644 index 00000000..3e485c26 --- /dev/null +++ b/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp @@ -0,0 +1,52 @@ +#include "weightfunctions/Distribution.h" + +namespace reglib +{ +Distribution::Distribution(){} +Distribution::~Distribution(){} + +void Distribution::train(std::vector & hist, unsigned int nr_bins){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} +void Distribution::update(){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} +double Distribution::getval(double x){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} +double Distribution::getcdf(double x){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} +void Distribution::print(){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} +void Distribution::setRegularization(double x){printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__); regularization = x;update();} +double Distribution::getNoise(){return 1;} +void Distribution::setNoise(double x){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} +void Distribution::getMaxdMind(double & maxd, double & mind, double prob){ + printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__); + double half = prob*0.5; + double minDist = 0; + double minDistScore = getcdf(mean+minDist); + double maxDist = 1; + double maxDistScore = getcdf(mean+maxDist); + + double target = 1-half; + //Grow to find interval + while(maxDistScore < target){ + minDist = maxDist; + minDistScore = maxDistScore; + maxDist *= 2; + maxDistScore = getcdf(mean+maxDist); + } + + //bisect to opt + for(unsigned int it = 0; it < 40; it++){ + double midDist = (minDist+maxDist)*0.5; + double midDistScore = getcdf(mean+midDist); + if(midDistScore > target){ + maxDist = midDist; + maxDistScore = midDistScore; + }else{ + minDist = midDist; + minDistScore = midDistScore; + } + if((maxDistScore-minDistScore) < 1e-15){break;} + } + + double midDist = (minDist+maxDist)*0.5; + mind = mean-midDist; + maxd = mean+midDist; +} + +} diff --git a/quasimodo/quasimodo_models/src/weightfunctions/GaussianDistribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/GaussianDistribution.cpp new file mode 100644 index 00000000..622c95f4 --- /dev/null +++ b/quasimodo/quasimodo_models/src/weightfunctions/GaussianDistribution.cpp @@ -0,0 +1,163 @@ +#include "weightfunctions/GaussianDistribution.h" + +namespace reglib +{ + + +GaussianDistribution::GaussianDistribution(bool refine_std_, bool zeromean_, bool refine_mean_, bool refine_mul_, double costpen_, int nr_refineiters_ ,double mul_, double mean_,double stdval_){ + refine_std = refine_std_; + zeromean = zeromean_; + refine_mean = refine_mean_; + refine_mul = refine_mul_; + costpen = costpen_; + nr_refineiters = nr_refineiters_; + + mul = mul_; + mean = mean_; + stdval = stdval_; + update(); +} +//GaussianDistribution::GaussianDistribution(double mul_, double mean_, double stdval_){ +// mul = mul_; +// mean = mean_; +// stdval = stdval_; +// update(); +//} +GaussianDistribution::~GaussianDistribution(){} + +double GaussianDistribution::scoreCurrent2(double mul, double mean, double stddiv, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + double info = -0.5/(stddiv*stddiv); + double sum = 0; + for(unsigned int i = 0; i < nr_data; i++){ + double dx = X[i] - mean; + double inp = info*dx*dx; + if(inp < cutoff_exp){sum += Y[i];} + else{ + double diff = mul*exp(info*dx*dx) - Y[i]; + if(diff > 0){ sum += costpen*diff;} + else{ sum -= diff;} + } + } + return sum; +} + +double GaussianDistribution::fitStdval2(double mul, double mean, double std_mid, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + int iter = 25; + double h = 0.000000001; + + double std_max = std_mid*2; + double std_min = 0; + for(int i = 0; i < iter; i++){ + std_mid = (std_max+std_min)/2; + double std_neg = scoreCurrent2(mul,mean,std_mid-h,X,Y,nr_data,costpen); + double std_pos = scoreCurrent2(mul,mean,std_mid+h,X,Y,nr_data,costpen); + if(std_neg < std_pos){ std_max = std_mid;} + else{ std_min = std_mid;} + } + return std_mid; +} + +double GaussianDistribution::fitMean2(double mul, double mean, double std_mid, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + int iter = 10; + double h = 0.000000001; + double mean_max = mean+1; + double mean_min = mean-1; + + // double mean_max = mean*2; + // double mean_min = 0; + + for(int i = 0; i < iter; i++){ + mean = (mean_max+mean_min)/2; + double std_neg = scoreCurrent2(mul,mean-h,std_mid,X,Y,nr_data,costpen); + double std_pos = scoreCurrent2(mul,mean+h,std_mid,X,Y,nr_data,costpen); + if(std_neg < std_pos){ mean_max = mean;} + else{ mean_min = mean;} + } + return mean; +} + +double GaussianDistribution::fitMul2(double mul, double mean, double std_mid, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + int iter = 25; + double h = 0.000000001; + double mul_max = mul*2; + double mul_min = 0; + + for(int i = 0; i < iter; i++){ + mul = (mul_max+mul_min)/2; + double std_neg = scoreCurrent2(mul-h,mean,std_mid,X,Y,nr_data,costpen); + double std_pos = scoreCurrent2(mul+h,mean,std_mid,X,Y,nr_data,costpen); + if(std_neg < std_pos){ mul_max = mul;} + else{ mul_min = mul;} + } + return mul; +} + +void GaussianDistribution::train(std::vector & hist, unsigned int nr_bins){ + if(nr_bins == 0){nr_bins = hist.size();} + mul = hist[0]; + mean = 0; + if(!zeromean){ + for(unsigned int k = 1; k < nr_bins; k++){ + if(hist[k] > mul){ + mul = hist[k]; + mean = k; + } + } + } + + std::vector X; + std::vector Y; + for(unsigned int k = 0; k < nr_bins; k++){ + if(hist[k] > mul*0.01){ + X.push_back(k); + Y.push_back(hist[k]); + } + } + + unsigned int nr_data_opt = X.size(); + + + double ysum = 0; + for(unsigned int i = 0; i < nr_data_opt; i++){ysum += fabs(Y[i]);} + + double std_mid = 0; + for(unsigned int i = 0; i < nr_data_opt; i++){std_mid += (X[i]-mean)*(X[i]-mean)*fabs(Y[i])/ysum;} + stdval = sqrt(std_mid); + + for(int i = 0; i < nr_refineiters; i++){ + if(refine_std){ stdval = fitStdval2( mul,mean,stdval,X,Y,nr_data_opt,costpen);} + if(refine_mean){ mean = fitMean2( mul,mean,stdval,X,Y,nr_data_opt,costpen);} + if(refine_mul){ mul = fitMul2( mul,mean,stdval,X,Y,nr_data_opt,costpen);} + } +} + +void GaussianDistribution::update(){ + scaledinformation = -0.5/((stdval+regularization)*(stdval+regularization)); +} + +double GaussianDistribution::getval(double x){ + double dx = mean-x; + return mul*exp(dx*dx*scaledinformation); +} +double GaussianDistribution::getcdf(double x){ + return 0.5 * erfc(-((x-mean)/(stdval+regularization)) * M_SQRT1_2); +} + +void GaussianDistribution::setNoise(double x){ + stdval = x; + update(); +} + +void GaussianDistribution::print(){ + printf("GaussianDistribution:: mul = %5.5f mean = %5.5f stdval = %5.5f reg = %5.5f\n",mul,mean,stdval,regularization); +} + +double GaussianDistribution::getNoise(){return stdval+regularization;} + +//void GaussianDistribution::getMaxdMind(double & maxd, double & mind, double prob){ + +//} + + +} + diff --git a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp new file mode 100644 index 00000000..12c26ac6 --- /dev/null +++ b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp @@ -0,0 +1,278 @@ +#include "weightfunctions/GeneralizedGaussianDistribution.h" +#include + +namespace reglib +{ + + +GeneralizedGaussianDistribution::GeneralizedGaussianDistribution(bool refine_std_, bool refine_power_, bool zeromean_, bool refine_mean_, bool refine_mul_, double costpen_, int nr_refineiters_ ,double mul_, double mean_,double stdval_,double power_){ + refine_std = refine_std_; + zeromean = zeromean_; + refine_mean = refine_mean_; + refine_mul = refine_mul_; + refine_power = refine_power_; + costpen = costpen_; + nr_refineiters = nr_refineiters_; + + mul = mul_; + mean = mean_; + stdval = stdval_; + power = power_; + update(); +} + +GeneralizedGaussianDistribution::~GeneralizedGaussianDistribution(){} + +//double GeneralizedGaussianDistribution::scoreCurrent2(double mul, double mean, double stddiv, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ +// double info = -0.5/(stddiv*stddiv); +// double sum = 0; +// for(unsigned int i = 0; i < nr_data; i++){ +// double dx = X[i] - mean; +// double inp = info*dx*dx; +// if(inp < cutoff_exp){sum += Y[i];} +// else{ +// double diff = mul*exp(info*dx*dx) - Y[i]; +// if(diff > 0){ sum += costpen*diff;} +// else{ sum -= diff;} +// } +// } +// return sum; +//} + +//double GeneralizedGaussianDistribution::fitStdval2(double mul, double mean, double std_mid, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ +// int iter = 25; +// double h = 0.000000001; + +// double std_max = std_mid*2; +// double std_min = 0; +// for(int i = 0; i < iter; i++){ +// std_mid = (std_max+std_min)/2; +// double std_neg = scoreCurrent2(mul,mean,std_mid-h,X,Y,nr_data,costpen); +// double std_pos = scoreCurrent2(mul,mean,std_mid+h,X,Y,nr_data,costpen); +// if(std_neg < std_pos){ std_max = std_mid;} +// else{ std_min = std_mid;} +// } +// return std_mid; +//} + +//double GeneralizedGaussianDistribution::fitMean2(double mul, double mean, double std_mid, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ +// int iter = 10; +// double h = 0.000000001; +// double mean_max = mean+1; +// double mean_min = mean-1; + +// for(int i = 0; i < iter; i++){ +// mean = (mean_max+mean_min)/2; +// double std_neg = scoreCurrent2(mul,mean-h,std_mid,X,Y,nr_data,costpen); +// double std_pos = scoreCurrent2(mul,mean+h,std_mid,X,Y,nr_data,costpen); +// if(std_neg < std_pos){ mean_max = mean;} +// else{ mean_min = mean;} +// } +// return mean; +//} + +double GeneralizedGaussianDistribution::fitMul3(double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + int iter = 25; + double h = 0.000000001; + double mul_max = mul*2; + double mul_min = 0; + + for(int i = 0; i < iter; i++){ + mul = (mul_max+mul_min)/2; + double std_neg = scoreCurrent3(mul-h,mean,std_mid,power,X,Y,nr_data,costpen); + double std_pos = scoreCurrent3(mul+h,mean,std_mid,power,X,Y,nr_data,costpen); + if(std_neg < std_pos){ mul_max = mul;} + else{ mul_min = mul;} + } + return mul; +} + + +double GeneralizedGaussianDistribution::scoreCurrent3(double mul, double mean, double stddiv, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + double sum = 0; + double invstd = 1.0/stddiv; + for(unsigned int i = 0; i < nr_data; i++){ + double dx = fabs(X[i] - mean)*invstd; + double inp = -0.5*pow(dx,power); + if(inp < cutoff_exp){sum += Y[i];} + else{ + double diff = Y[i]*Y[i]*(mul*exp(inp) - Y[i]); + if(diff > 0){ sum += costpen*diff;} + else{ sum -= diff;} + } + } + return sum; +} + +double GeneralizedGaussianDistribution::fitStdval3(double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + int iter = 25; + double h = 0.000000001; + + double std_max = std_mid*2; + double std_min = 0; + for(int i = 0; i < iter; i++){ + std_mid = (std_max+std_min)/2; + double std_neg = scoreCurrent3(mul,mean,std_mid-h,power,X,Y,nr_data,costpen); + double std_pos = scoreCurrent3(mul,mean,std_mid+h,power,X,Y,nr_data,costpen); + if(std_neg < std_pos){ std_max = std_mid;} + else{ std_min = std_mid;} + } + return std_mid; +} + +double GeneralizedGaussianDistribution::fitPower3(double current_mul, double current_mean, double current_std, double current_power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + int iter = 20; + double h = 0.000000001; + + double power_max = current_power*2; + double power_min = 0.001; + for(int i = 0; i < iter; i++){ + current_power = (power_max+power_min)/2; + double std_neg = scoreCurrent3(current_mul,current_mean,current_std,current_power-h,X,Y,nr_data,costpen); + double std_pos = scoreCurrent3(current_mul,current_mean,current_std,current_power+h,X,Y,nr_data,costpen); + if(std_neg < std_pos){ power_max = current_power;} + else{ power_min = current_power;} + } + current_power = (power_max+power_min)/2; + return current_power; +} + +double GeneralizedGaussianDistribution::fitMean3(double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ + int iter = 10; + double h = 0.000000001; + double mean_max = mean+10; + double mean_min = mean-10; + + for(int i = 0; i < iter; i++){ + mean = (mean_max+mean_min)/2; + double std_neg = scoreCurrent3(mul,mean-h,std_mid,power,X,Y,nr_data,costpen); + double std_pos = scoreCurrent3(mul,mean+h,std_mid,power,X,Y,nr_data,costpen); + if(std_neg < std_pos){ mean_max = mean;} + else{ mean_min = mean;} + } + return mean; +} + +void GeneralizedGaussianDistribution::train(std::vector & hist, unsigned int nr_bins){ + //printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__); + if(nr_bins == 0){nr_bins = hist.size();} + mul = hist[0]; + mean = 0; + if(!zeromean){ + for(unsigned int k = 1; k < nr_bins; k++){ + if(hist[k] > mul){ + mul = hist[k]; + mean = k; + } + } + } + + std::vector X; + std::vector Y; + for(unsigned int k = 0; k < nr_bins; k++){ + if(hist[k] > mul*0.001){ + X.push_back(k); + Y.push_back(hist[k]); + } + } + + unsigned int nr_data_opt = X.size(); + + + double ysum = 0; + for(unsigned int i = 0; i < nr_data_opt; i++){ysum += fabs(Y[i]);} + + double std_mid = 0; + for(unsigned int i = 0; i < nr_data_opt; i++){std_mid += (X[i]-mean)*(X[i]-mean)*fabs(Y[i])/ysum;} + stdval = sqrt(std_mid); + + for(int i = 0; i < nr_refineiters; i++){ + if(refine_std){ stdval = fitStdval3( mul,mean,stdval,power,X,Y,nr_data_opt,costpen);} + if(refine_mean){ mean = fitMean3( mul,mean,stdval,power,X,Y,nr_data_opt,costpen);} + if(refine_mul){ mul = fitMul3( mul,mean,stdval,power,X,Y,nr_data_opt,costpen);} + if(refine_power){ power = fitPower3( mul,mean,stdval,power,X,Y,nr_data_opt,costpen);} + } + + print(); +} + +void GeneralizedGaussianDistribution::update(){ + //printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__); + update_numcdf_vec(); +} + +double GeneralizedGaussianDistribution::getInp(double x){ + return -0.5*pow(fabs(mean-x)/(stdval+regularization),power); +} + +double GeneralizedGaussianDistribution::getval(double x){ + return mul*exp(getInp(x)); +} +double GeneralizedGaussianDistribution::getcdf(double x){ + + double part = (x-start)/(stop-start); + if(part < 0){return 0;} + if(part >= 1){return 1;} + part *= double(numcdf_vec.size()-1); + unsigned int id0 = part; + unsigned int id1 = id0+1; + double w0 = double(id1)-part; + double w1 = part-double(id0); + //printf("x: %f start: %f stop: %f part: %f\n",x,start,stop,part); + //printf("id: %5.5i %5.5i w: %5.5f %5.5f part: %5.5f\n",id0,id1,w0,w1,part); + //double gt = 0.5 * erfc(-((x-mean)/(stdval+regularization)) * M_SQRT1_2); + //printf("gt: %f est: %f err: %f\n",gt,cdfx,cdfx-gt); + double cdfx = numcdf_vec[id0]*w0 + numcdf_vec[id1]*w1; + return cdfx; +} + +void GeneralizedGaussianDistribution::setNoise(double x){ + stdval = x; + update(); +} + +void GeneralizedGaussianDistribution::print(){ + printf("GeneralizedGaussianDistribution:: mul = %5.5f mean = %5.5f stdval = %5.5f reg = %5.5f power = %5.5f\n",mul,mean,stdval,regularization,power); +} + +double GeneralizedGaussianDistribution::getNoise(){return stdval+regularization;} + +void GeneralizedGaussianDistribution::update_numcdf_vec(unsigned int bins, double prob){ +// printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__); +// start = mean; +// while(getval(start) > prob){start -= 0.01;} + +// stop = mean; +// while(getval(stop) > prob){stop += 0.01;} +// printf("num: %10.10f %10.10f\n",start,stop); + + start = mean - pow(-2.0*log(prob),1.0/power)*stdval; + stop = mean + pow(-2.0*log(prob),1.0/power)*stdval; + +// printf("ana: %10.10f %10.10f\n",start,stop); +// exit(0); + double step = (stop-start)/(bins-1); + double sum = 0; + numcdf_vec.resize(bins); + for(unsigned int i = 0; i < bins; i++){ + double x = start+double(i)*step+0.5*step; + numcdf_vec[i] = sum; + sum += getval(x); + } + for(unsigned int i = 0; i < bins; i++){numcdf_vec[i] /= sum;} +// for(unsigned int i = 0; i < bins; i++){ +// double x = start+double(i)*step; +// printf("------------------\n%i -> ",i); +// double cdfx = getcdf(x); +// double gt = 0.5 * erfc(-((x-mean)/(stdval+regularization)) * M_SQRT1_2); +// printf("gt: %f est: %f err: %f\n",gt,cdfx,cdfx-gt); +// } +//exit(0); +// for(unsigned int i = 0; i < bins; i++){ +// printf("%5.5i -> %5.5f -> %10.10f\n",i,start+double(i)*step,numcdf_vec[i]); +// } +} + + +} + diff --git a/quasimodo/quasimodo_models/src/weightfunctions/SignalProcessing.cpp b/quasimodo/quasimodo_models/src/weightfunctions/SignalProcessing.cpp new file mode 100644 index 00000000..766d216f --- /dev/null +++ b/quasimodo/quasimodo_models/src/weightfunctions/SignalProcessing.cpp @@ -0,0 +1,59 @@ +#include "weightfunctions/SignalProcessing.h" + +namespace reglib +{ + +SignalProcessing::SignalProcessing(){} +SignalProcessing::~SignalProcessing(){} +void SignalProcessing::process(std::vector & in, std::vector & out, double stdval, unsigned int nr_bins){ + if(nr_bins == 0){nr_bins = in.size();} + + + int nr_bins2 = 2*nr_bins; + + std::vector tmphist; + tmphist.resize(nr_bins2); + + std::vector tmphist_blur; + tmphist_blur.resize(nr_bins2); + + for(int i = 0; i < nr_bins; i++){ + tmphist[i] = in[nr_bins-1-i]; + tmphist[nr_bins+i] = in[i]; + tmphist_blur[i] = 0; + tmphist_blur[nr_bins+i] = 0; + } + + double info = -0.5/(stdval*stdval); + double weights[nr_bins2]; + for(int i = 0; i < nr_bins2; i++){weights[i] = 0;} + for(int i = 0; i < nr_bins2; i++){ + double current = exp(i*i*info); + weights[i] = current; + if(current < 0.001){break;} + } + + int offset = 4.0*stdval; + offset = std::max(4,offset); + for(int i = 0; i < nr_bins2; i++){ + double v = tmphist[i]; + if(v == 0){continue;} + + int start = std::max(0,i-offset); + int stop = std::min(nr_bins2,i+offset+1); + + for(int j = start; j < stop; j++){tmphist_blur[j] += v*weights[abs(i-j)];} + } + for(int i = 0; i < nr_bins; i++){out[i] = tmphist_blur[i+nr_bins];} + + float bef = 0; + float aft = 0; + for(int i = 0; i < nr_bins; i++){ + bef += in[i]; + aft += out[i]; + } + + for(int i = 0; i < nr_bins; i++){out[i] *= bef/aft;} +} + +} From d58385d1315a61646f8706eb717dbb9069b4410a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 20 Sep 2016 17:20:06 +0200 Subject: [PATCH 105/255] ... --- .../include/weightfunctions/Distribution.h | 1 + .../src/modelupdater/ModelUpdater.cpp | 5 +- .../DistanceWeightFunction2PPR3.cpp | 450 ++++++++---------- .../src/weightfunctions/Distribution.cpp | 2 +- .../GeneralizedGaussianDistribution.cpp | 77 +-- 5 files changed, 215 insertions(+), 320 deletions(-) diff --git a/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h b/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h index 9ae533ce..5f17e13e 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h @@ -19,6 +19,7 @@ class Distribution Distribution(); ~Distribution(); + virtual void reset(); virtual void train(std::vector & hist, unsigned int nr_bins = 0); virtual void update(); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index a4c71e30..adc6cb67 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1955,7 +1955,7 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & //printf("inlierratio: %f nr_points: %i nr rep: %i\n",inlierratio,framesp1.size(),rr_vec.size()); unsigned long nr_rr = rr_vec.size(); - printf("getDynamicWeights nr_rr: %i\n",nr_rr); + //printf("getDynamicWeights nr_rr: %i\n",nr_rr); pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); @@ -2116,7 +2116,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s nfunc->computeModel(nvec); //exit(0); - int frameConnections = 0; + long frameConnections = 0; std::vector< std::vector< std::vector > > pixel_weights; pcl::PointCloud::Ptr cloud (new pcl::PointCloud); @@ -2172,7 +2172,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for(unsigned int w = 0; w < width;w++){ int ind = h*width+w; - valids[offset+ind] = detdata[ind] == 0; float p_moving = current_occlusions[ind]; diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp index df0adb38..3ad29bad 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp @@ -7,82 +7,82 @@ #include double getTime3(){ - struct timeval start1; - gettimeofday(&start1, NULL); - return double(start1.tv_sec+(start1.tv_usec/1000000.0)); + struct timeval start1; + gettimeofday(&start1, NULL); + return double(start1.tv_sec+(start1.tv_usec/1000000.0)); } namespace reglib{ DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3(Distribution * dist_, double maxd_, int histogram_size_){ - fixed_histogram_size = false; + fixed_histogram_size = false; - regularization = 0.1; - maxd = maxd_; - mind = 0; - histogram_size = histogram_size_; - starthistogram_size = histogram_size; - stdval = blurval; + regularization = 0.1; + maxd = maxd_; + mind = 0; + histogram_size = histogram_size_; + starthistogram_size = histogram_size; + stdval = blurval; - maxnoise = 99999999999999; - noiseval = 100.0; + maxnoise = 99999999999999; + noiseval = 100.0; - prob.resize(histogram_size+1); - prob[histogram_size] = 0; + prob.resize(histogram_size+1); + prob[histogram_size] = 0; - infront.resize(histogram_size+1); - infront[histogram_size] = 1; + infront.resize(histogram_size+1); + infront[histogram_size] = 1; - histogram.resize(histogram_size+1); - blur_histogram.resize(histogram_size+1); - noise.resize(histogram_size+1); - noisecdf.resize(histogram_size+1); + histogram.resize(histogram_size+1); + blur_histogram.resize(histogram_size+1); + noise.resize(histogram_size+1); + noisecdf.resize(histogram_size+1); - mulval = 1; - meanval = 0; + mulval = 1; + meanval = 0; - startmaxd = maxd_; + startmaxd = maxd_; - scale_convergence = true; - nr_inliers = 1; + scale_convergence = true; + nr_inliers = 1; - target_length = 10.0; + target_length = 10.0; - bidir = false; - iter = 0; + bidir = false; + iter = 0; - maxp = 0.99; + maxp = 0.99; - first = true; + first = true; - startreg = 2; - blurval = 4; - stdval = 100; - noiseval = 100.0; - debugg_print = true; - threshold = false; - blur = 0.03; - data_per_bin = 30; + startreg = 2; + blurval = 4; + stdval = 100; + noiseval = 100.0; + debugg_print = true; + threshold = false; + blur = 0.03; + data_per_bin = 30; - update_size = true;//(setting&2 != 1); - max_under_mean = true;//(setting&1 != 0); - interp = true;//(setting&1 != 0); + update_size = true;//(setting&2 != 1); + max_under_mean = true;//(setting&1 != 0); + interp = true;//(setting&1 != 0); - nr_refineiters = 1; - convergence_threshold = 0.05; - compute_infront = false; + nr_refineiters = 1; + convergence_threshold = 0.05; + compute_infront = false; - sp = new SignalProcessing(); - dist = dist_;//new GeneralizedGaussianDistribution(true,true);//GaussianDistribution();//GeneralizedGaussianDistribution();//GaussianDistribution(); + sp = new SignalProcessing(); + dist = dist_;//new GeneralizedGaussianDistribution(true,true);//GaussianDistribution();//GeneralizedGaussianDistribution();//GaussianDistribution(); } DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3( double maxd_, int histogram_size_){ - fixed_histogram_size = false; + fixed_histogram_size = false; regularization = 0.1; maxd = maxd_; @@ -91,7 +91,7 @@ DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3( double maxd_, int hist starthistogram_size = histogram_size; stdval = blurval; - maxnoise = 99999999999999; + maxnoise = 99999999999999; noiseval = 100.0; prob.resize(histogram_size+1); @@ -128,22 +128,22 @@ DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3( double maxd_, int hist blurval = 4; stdval = 100; noiseval = 100.0; - debugg_print = true; + debugg_print = true; threshold = false; blur = 0.03; data_per_bin = 30; update_size = true;//(setting&2 != 1); - max_under_mean = true;//(setting&1 != 0); + max_under_mean = true;//(setting&1 != 0); interp = true;//(setting&1 != 0); nr_refineiters = 1; convergence_threshold = 0.05; compute_infront = false; - sp = new SignalProcessing(); - dist = new GeneralizedGaussianDistribution(true,true);//GaussianDistribution();//GeneralizedGaussianDistribution();//GaussianDistribution(); + sp = new SignalProcessing(); + dist = new GeneralizedGaussianDistribution(true,true);//GaussianDistribution();//GeneralizedGaussianDistribution();//GaussianDistribution(); } DistanceWeightFunction2PPR3::~DistanceWeightFunction2PPR3(){ @@ -154,162 +154,157 @@ DistanceWeightFunction2PPR3::~DistanceWeightFunction2PPR3(){ double DistanceWeightFunction2PPR3::getNoise(){return regularization+noiseval;} void DistanceWeightFunction2PPR3::recomputeHistogram(std::vector & hist, MatrixXd & mat){ - const unsigned int nr_data = mat.cols(); - const int nr_dim = mat.rows(); - histogram_mul2 = double(histogram_size)/(maxd-mind); - for(int j = 0; j < histogram_size; j++){hist[j] = 0;} - for(unsigned int j = 0; j < nr_data; j++){ - for(int k = 0; k < nr_dim; k++){ - double ind = getInd(mat(k,j)); - if(ind >= 0 && (ind+0.00001) < histogram_size){ - hist[int(ind+0.00001)]++; - } - } - } + const unsigned int nr_data = mat.cols(); + const int nr_dim = mat.rows(); + histogram_mul2 = double(histogram_size)/(maxd-mind); + for(int j = 0; j < histogram_size; j++){hist[j] = 0;} + for(unsigned int j = 0; j < nr_data; j++){ + for(int k = 0; k < nr_dim; k++){ + double ind = getInd(mat(k,j)); + if(ind >= 0 && (ind+0.00001) < histogram_size){ + hist[int(ind+0.00001)]++; + } + } + } } void DistanceWeightFunction2PPR3::recomputeProbs(){ - for(int k = 0; k < histogram_size; k++){noise[k] = dist->getval(k);} - if(compute_infront){ - for(int k = 0; k < histogram_size; k++){noisecdf[k] = dist->getcdf(k);} - } + for(int k = 0; k < histogram_size; k++){noise[k] = dist->getval(k);} + if(compute_infront){ + for(int k = 0; k < histogram_size; k++){noisecdf[k] = dist->getcdf(k);} + } double maxhist = blur_histogram[0]; for(int k = 1; k < histogram_size; k++){maxhist = std::max(maxhist,double(blur_histogram[k]));} double minhist = maxhist*0.01; - - for(int k = 0; k < histogram_size; k++){ - if(max_under_mean && k < dist->mean){ prob[k] = maxp; } - else{ + for(int k = 0; k < histogram_size; k++){ + if(max_under_mean && k < dist->mean){ prob[k] = maxp; } + else{ double hs = std::max(minhist,std::max(1.0,double(blur_histogram[k]))); - prob[k] = std::min(maxp , noise[k]/hs);//never fully trust any data - } - infront[k] = (1-prob[k])*noisecdf[k]; - } + prob[k] = std::min(maxp , noise[k]/hs);//never fully trust any data + } + infront[k] = (1-prob[k])*noisecdf[k]; + } } void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){ - const unsigned int nr_data = mat.cols(); - const int nr_dim = mat.rows(); - double start_time; - - if(debugg_print){printf("\n################################################### ITER:%i ############################################################\n",iter);} - if(!fixed_histogram_size){ - if(first){ - first = false; - double sum = 0; - for(unsigned int j = 0; j < nr_data; j++){ - for(int k = 0; k < nr_dim; k++){sum += mat(k,j)*mat(k,j);} - } - double stdval = sqrt(sum / double(nr_data*nr_dim)); - if(debugg_print){printf("stdval: %f\n",stdval);} - dist->setNoise(stdval); - } - - if(update_size){ - double next_maxd; - double next_mind; - start_time = getTime(); - dist->getMaxdMind(next_maxd,next_mind,0.05); - next_maxd *= (maxd-mind)/double(histogram_size); - next_mind *= (maxd-mind)/double(histogram_size); - if(debugg_print){printf("getMaxdMind time: %5.5fs\n",getTime()-start_time);} - if(!bidir){next_mind = 0;} - mind = next_mind; - maxd = next_maxd; - - start_time = getTime(); - double nr_inside = 0; - for(unsigned int j = 0; j < nr_data; j++){ - for(int k = 0; k < nr_dim; k++){ - double d = mat(k,j); - if(mind < d && d < maxd){nr_inside++;} - } - } - if(debugg_print){printf("nr_inside time: %5.5fs\n",getTime()-start_time);} - - int new_histogram_size = std::min(int(histogram.size()),std::max(50,int(0.5 + nr_inside/data_per_bin))); - histogram_size = std::min(1000,new_histogram_size); - blurval = blur*double(histogram_size)*float(histogram_size)/float(new_histogram_size); - if(debugg_print){printf("nr_inside: %f histogram_size: %i blurval: %f\n",nr_inside,histogram_size,blurval);} - } - } - - if(debugg_print){printf("histogram_size: %5.5f maxd: %5.5f mind: %5.5f\n",double(histogram_size),maxd,mind);} - - start_time = getTime(); - recomputeHistogram(histogram,mat); - if(debugg_print){printf("computing histogram time: %5.5fs\n",getTime()-start_time);} - - - start_time = getTime(); - sp->process(histogram,blur_histogram,blurval,histogram_size); - if(debugg_print){printf("SignalProcessing time: %5.5fs\n",getTime()-start_time);} - - - start_time = getTime(); - dist->setRegularization(double(histogram_size)*regularization/(maxd-mind)); - dist->train(blur_histogram,histogram_size); - dist->update(); - if(debugg_print){printf("train time: %5.5fs\n",getTime()-start_time);dist->print();} - - start_time = getTime(); - recomputeProbs(); - if(debugg_print){printf("recomputeProbs time: %5.5fs\n",getTime()-start_time);} - - start_time = getTime(); - getProbs(mat); - if(debugg_print){printf("get number of inliers time: %5.5fs nr_inliers: %f\n",getTime()-start_time,nr_inliers);} + const unsigned int nr_data = mat.cols(); + const int nr_dim = mat.rows(); + double start_time; + + if(debugg_print){printf("\n%%################################################### ITER:%i ############################################################\n",iter);} + if(!fixed_histogram_size){ + if(first){ + first = false; + double sum = 0; + for(unsigned int j = 0; j < nr_data; j++){ + for(int k = 0; k < nr_dim; k++){sum += mat(k,j)*mat(k,j);} + } + double stdval = sqrt(sum / double(nr_data*nr_dim)); + if(debugg_print){printf("%%stdval: %f\n",stdval);} + dist->setNoise(stdval); + } + + if(update_size){ + double next_maxd; + double next_mind; + start_time = getTime(); + dist->getMaxdMind(next_maxd,next_mind,0.001); + next_maxd *= (maxd-mind)/double(histogram_size); + next_mind *= (maxd-mind)/double(histogram_size); + if(debugg_print){printf("%%getMaxdMind time: %5.5fs\n",getTime()-start_time);} + if(!bidir){next_mind = 0;} + mind = next_mind; + maxd = next_maxd; + + start_time = getTime(); + double nr_inside = 0; + for(unsigned int j = 0; j < nr_data; j++){ + for(int k = 0; k < nr_dim; k++){ + double d = mat(k,j); + if(mind < d && d < maxd){nr_inside++;} + } + } + if(debugg_print){printf("%%nr_inside time: %5.5fs\n",getTime()-start_time);} + + int new_histogram_size = std::min(int(histogram.size()),std::max(50,int(0.5 + nr_inside/data_per_bin))); + histogram_size = std::min(1000,new_histogram_size); + blurval = blur*double(histogram_size)*float(histogram_size)/float(new_histogram_size); + if(debugg_print){printf("%%nr_inside: %f histogram_size: %i blurval: %f\n",nr_inside,histogram_size,blurval);} + } + } + + if(debugg_print){printf("%%histogram_size: %5.5f maxd: %5.5f mind: %5.5f\n",double(histogram_size),maxd,mind);} + start_time = getTime(); + recomputeHistogram(histogram,mat); + if(debugg_print){printf("%%computing histogram time: %5.5fs\n",getTime()-start_time);} + + start_time = getTime(); + sp->process(histogram,blur_histogram,blurval,histogram_size); + if(debugg_print){printf("%%SignalProcessing time: %5.5fs\n",getTime()-start_time);} + + start_time = getTime(); + dist->setRegularization(double(histogram_size)*regularization/(maxd-mind)); + dist->train(blur_histogram,histogram_size); + dist->update(); + if(debugg_print){printf("%%train time: %5.5fs\n",getTime()-start_time);dist->print();} + + start_time = getTime(); + recomputeProbs(); + if(debugg_print){printf("%%recomputeProbs time: %5.5fs\n",getTime()-start_time);} + + start_time = getTime(); + getProbs(mat); + if(debugg_print){printf("%%get number of inliers time: %5.5fs nr_inliers: %f\n",getTime()-start_time,nr_inliers);} if(debugg_print){printf("hist = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(histogram[k]));} printf("];\n");} if(debugg_print){printf("hist_smooth = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(blur_histogram[k]));} printf("];\n");} - if(debugg_print){printf("noise = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(noise[k]));} printf("];\n");} - if(debugg_print){printf("noisecdf = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",int(noisecdf[k]));} printf("];\n");} - if(debugg_print){printf("prob = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",prob[k]);} printf("];\n");} - if(debugg_print){printf("infront = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",infront[k]);} printf("];\n");} - if(debugg_print){printf("clf; hold on; plot(hist_smooth,'r','LineWidth',2); plot(hist,'m','LineWidth',2); plot(noise,'b','LineWidth',2); plot(prob*max(noise),'g','LineWidth',2); plot(infront*max(noise),'g','LineWidth',2);\n");} - -// if(false){printf("meanoffset: %f stdval2: %f stdval: %f regularization: %f\n",meanval2,stdval2,noiseval,regularization);} - if(!fixed_histogram_size && update_size ){ - - double next_maxd; - double next_mind; - start_time = getTime(); - - dist->getMaxdMind(next_maxd,next_mind,0.05); - next_maxd *= (maxd-mind)/double(histogram_size); - next_mind *= (maxd-mind)/double(histogram_size); - if(debugg_print){ - printf("getMaxdMind time: %5.5fs\n",getTime()-start_time); - printf("current: %f %f\n",mind,maxd); - printf("next: %f %f\n",next_mind,next_maxd); - } - - if(!bidir){next_mind = 0;} - - //Ensure overlap - if(next_maxd < mind){ return;} - if(next_mind > maxd){ return;} - if(next_maxd == next_mind){ return;} - - if(debugg_print){printf("next: %f %f\n",next_mind,next_maxd);} - double overlap = 0; - if(next_maxd <= maxd && next_mind >= mind){ overlap = next_maxd-next_mind;} + if(debugg_print){printf("noise = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(noise[k]));} printf("];\n");} + if(debugg_print){printf("noisecdf = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",int(noisecdf[k]));} printf("];\n");} + if(debugg_print){printf("prob = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",prob[k]);} printf("];\n");} + if(debugg_print){printf("infront = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",infront[k]);} printf("];\n");} + if(debugg_print){printf("clf; hold on; plot(hist_smooth,'r','LineWidth',2); plot(hist,'m','LineWidth',2); plot(noise,'b','LineWidth',2); plot(prob*max(noise),'g','LineWidth',2); plot(infront*max(noise),'g','LineWidth',2);\n");} + + if(!fixed_histogram_size && update_size ){ + + double next_maxd; + double next_mind; + start_time = getTime(); + + dist->getMaxdMind(next_maxd,next_mind,0.001); + next_maxd *= (maxd-mind)/double(histogram_size); + next_mind *= (maxd-mind)/double(histogram_size); + if(debugg_print){ + printf("%%getMaxdMind time: %5.5fs\n",getTime()-start_time); + printf("%%current: %f %f\n",mind,maxd); + printf("%%next: %f %f\n",next_mind,next_maxd); + } + + if(!bidir){next_mind = 0;} + + //Ensure overlap + if(next_maxd < mind){ return;} + if(next_mind > maxd){ return;} + if(next_maxd == next_mind){ return;} + + if(debugg_print){printf("%%next: %f %f\n",next_mind,next_maxd);} + double overlap = 0; + if(next_maxd <= maxd && next_mind >= mind){ overlap = next_maxd-next_mind;} else if(next_maxd >= maxd && next_mind <= mind){overlap = maxd- mind;} else if(next_maxd <= maxd && next_mind <= mind){overlap = next_maxd- mind;} - else if(next_maxd >= maxd && next_mind >= mind){overlap = maxd-next_mind;} - - double ratio = 2*overlap/(maxd+next_maxd-mind-next_mind); - double newlogdiff = log(ratio); - if(debugg_print){printf("overlap: %5.5f ratio: %5.5f newlogdiff: %5.5f\n",overlap,ratio,newlogdiff);} - - if(fabs(newlogdiff) > 0.2 && iter < 3){ - iter++; - computeModel(mat); - return; - }else{iter = 0;} - } + else if(next_maxd >= maxd && next_mind >= mind){overlap = maxd-next_mind;} + + double ratio = 2*overlap/(maxd+next_maxd-mind-next_mind); + double newlogdiff = log(ratio); + if(debugg_print){printf("%%overlap: %5.5f ratio: %5.5f newlogdiff: %5.5f\n",overlap,ratio,newlogdiff);} + + if(fabs(newlogdiff) > 0.2 && iter < 3){ + iter++; + computeModel(mat); + return; + }else{iter = 0;} + } } VectorXd DistanceWeightFunction2PPR3::getProbs(MatrixXd mat){ @@ -381,62 +376,37 @@ double DistanceWeightFunction2PPR3::getProbInfront(double d, bool debugg){ } bool DistanceWeightFunction2PPR3::update(){ -// if(true){ - std::vector new_histogram = histogram; - std::vector new_blur_histogram = blur_histogram; - - float old_sum_prob = 0; - for(int k = 0; k < histogram_size; k++){old_sum_prob += prob[k] * histogram[k];} - -// Gaussian3 g = Gaussian3(mulval,meanval,stdval);//getModel(stdval,blur_histogram,uniform_bias); - - int iteration = 0; - double regularization_before = regularization; - for(int i = 0; i < 500; i++){ - iteration++; - regularization *= 0.5; - double before_noise = dist->getNoise(); - dist->setRegularization(regularization); - double after_noise = dist->getNoise(); - - if(0.99*before_noise < after_noise){return true;} + double start_time; - float new_sum_prob = 0; - for(int k = 0; k < histogram_size; k++){ - double hs = new_blur_histogram[k] +0.0000001; - new_sum_prob += std::min(maxp , dist->getval(k)/hs) * new_histogram[k]; - } + double old_sum_prob = 0; + for(int k = 0; k < histogram_size; k++){old_sum_prob += prob[k] * histogram[k];} + int iteration = 0; + double regularization_before = regularization; + for(int i = 0; i < 500; i++){ + iteration++; + regularization *= 0.5; - //printf("new_sum_prob %f old_sum_prob %f ratio: %f\n",new_sum_prob,old_sum_prob,new_sum_prob/old_sum_prob); - if(new_sum_prob < 0.99*old_sum_prob || regularization*10 < regularization_before){return false;} + start_time = getTime(); + double before_noise = dist->getNoise(); + dist->setRegularization(double(histogram_size)*regularization/(maxd-mind)); + dist->update(); + double after_noise = dist->getNoise(); + if(debugg_print){printf("%%train time: %5.5fs\n",getTime()-start_time);dist->print();} - //recomputeProbs(); + start_time = getTime(); + recomputeProbs(); + if(debugg_print){printf("%%recomputeProbs time: %5.5fs\n",getTime()-start_time);} + if(0.99*before_noise < after_noise){return true;} - //double change = histogram_size*regularization/(maxd-mind); -// //printf("DistanceWeightFunction2PPR3::update() %i -> reg %f change %f\n",i,regularization,change); + double new_sum_prob = 0; + for(int k = 0; k < histogram_size; k++){new_sum_prob += prob[k] * histogram[k];} - //if(change < 0.01*dist->getNoise()){return true;} - -// g.stdval += change; -// g.update(); - -// float new_sum_prob = 0; -// for(int k = 0; k < histogram_size; k++){ -// double hs = new_blur_histogram[k] +0.0000001; -// new_sum_prob += std::min(maxp , g.getval(k)/hs) * new_histogram[k]; -// } - -// //printf("new_sum_prob %f old_sum_prob %f ratio: %f\n",new_sum_prob,old_sum_prob,new_sum_prob/old_sum_prob); -// if(new_sum_prob < 0.99*old_sum_prob || regularization < regularization_before/double(target_length*5)){return false;} -// g.stdval -= change; -// g.update(); - } - return true; -// }else{ -// regularization *= 0.5; -// } + //printf("new_sum_prob %f old_sum_prob %f ratio: %f\n",new_sum_prob,old_sum_prob,new_sum_prob/old_sum_prob); + if(new_sum_prob < 0.99*old_sum_prob || regularization*10 < regularization_before){return false;} + } + return true; } void DistanceWeightFunction2PPR3::reset(){ diff --git a/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp index 3e485c26..aead55d1 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp @@ -4,7 +4,7 @@ namespace reglib { Distribution::Distribution(){} Distribution::~Distribution(){} - +void Distribution::reset(){printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__);} void Distribution::train(std::vector & hist, unsigned int nr_bins){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} void Distribution::update(){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} double Distribution::getval(double x){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} diff --git a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp index 12c26ac6..5eb89329 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp @@ -4,7 +4,6 @@ namespace reglib { - GeneralizedGaussianDistribution::GeneralizedGaussianDistribution(bool refine_std_, bool refine_power_, bool zeromean_, bool refine_mean_, bool refine_mul_, double costpen_, int nr_refineiters_ ,double mul_, double mean_,double stdval_,double power_){ refine_std = refine_std_; zeromean = zeromean_; @@ -23,54 +22,6 @@ GeneralizedGaussianDistribution::GeneralizedGaussianDistribution(bool refine_std GeneralizedGaussianDistribution::~GeneralizedGaussianDistribution(){} -//double GeneralizedGaussianDistribution::scoreCurrent2(double mul, double mean, double stddiv, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ -// double info = -0.5/(stddiv*stddiv); -// double sum = 0; -// for(unsigned int i = 0; i < nr_data; i++){ -// double dx = X[i] - mean; -// double inp = info*dx*dx; -// if(inp < cutoff_exp){sum += Y[i];} -// else{ -// double diff = mul*exp(info*dx*dx) - Y[i]; -// if(diff > 0){ sum += costpen*diff;} -// else{ sum -= diff;} -// } -// } -// return sum; -//} - -//double GeneralizedGaussianDistribution::fitStdval2(double mul, double mean, double std_mid, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ -// int iter = 25; -// double h = 0.000000001; - -// double std_max = std_mid*2; -// double std_min = 0; -// for(int i = 0; i < iter; i++){ -// std_mid = (std_max+std_min)/2; -// double std_neg = scoreCurrent2(mul,mean,std_mid-h,X,Y,nr_data,costpen); -// double std_pos = scoreCurrent2(mul,mean,std_mid+h,X,Y,nr_data,costpen); -// if(std_neg < std_pos){ std_max = std_mid;} -// else{ std_min = std_mid;} -// } -// return std_mid; -//} - -//double GeneralizedGaussianDistribution::fitMean2(double mul, double mean, double std_mid, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ -// int iter = 10; -// double h = 0.000000001; -// double mean_max = mean+1; -// double mean_min = mean-1; - -// for(int i = 0; i < iter; i++){ -// mean = (mean_max+mean_min)/2; -// double std_neg = scoreCurrent2(mul,mean-h,std_mid,X,Y,nr_data,costpen); -// double std_pos = scoreCurrent2(mul,mean+h,std_mid,X,Y,nr_data,costpen); -// if(std_neg < std_pos){ mean_max = mean;} -// else{ mean_min = mean;} -// } -// return mean; -//} - double GeneralizedGaussianDistribution::fitMul3(double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ int iter = 25; double h = 0.000000001; @@ -154,7 +105,6 @@ double GeneralizedGaussianDistribution::fitMean3(double mul, double mean, double } void GeneralizedGaussianDistribution::train(std::vector & hist, unsigned int nr_bins){ - //printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__); if(nr_bins == 0){nr_bins = hist.size();} mul = hist[0]; mean = 0; @@ -193,7 +143,7 @@ void GeneralizedGaussianDistribution::train(std::vector & hist, unsigned if(refine_power){ power = fitPower3( mul,mean,stdval,power,X,Y,nr_data_opt,costpen);} } - print(); + //print(); } void GeneralizedGaussianDistribution::update(){ @@ -218,10 +168,6 @@ double GeneralizedGaussianDistribution::getcdf(double x){ unsigned int id1 = id0+1; double w0 = double(id1)-part; double w1 = part-double(id0); - //printf("x: %f start: %f stop: %f part: %f\n",x,start,stop,part); - //printf("id: %5.5i %5.5i w: %5.5f %5.5f part: %5.5f\n",id0,id1,w0,w1,part); - //double gt = 0.5 * erfc(-((x-mean)/(stdval+regularization)) * M_SQRT1_2); - //printf("gt: %f est: %f err: %f\n",gt,cdfx,cdfx-gt); double cdfx = numcdf_vec[id0]*w0 + numcdf_vec[id1]*w1; return cdfx; } @@ -238,19 +184,9 @@ void GeneralizedGaussianDistribution::print(){ double GeneralizedGaussianDistribution::getNoise(){return stdval+regularization;} void GeneralizedGaussianDistribution::update_numcdf_vec(unsigned int bins, double prob){ -// printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__); -// start = mean; -// while(getval(start) > prob){start -= 0.01;} - -// stop = mean; -// while(getval(stop) > prob){stop += 0.01;} -// printf("num: %10.10f %10.10f\n",start,stop); - start = mean - pow(-2.0*log(prob),1.0/power)*stdval; stop = mean + pow(-2.0*log(prob),1.0/power)*stdval; -// printf("ana: %10.10f %10.10f\n",start,stop); -// exit(0); double step = (stop-start)/(bins-1); double sum = 0; numcdf_vec.resize(bins); @@ -260,17 +196,6 @@ void GeneralizedGaussianDistribution::update_numcdf_vec(unsigned int bins, doubl sum += getval(x); } for(unsigned int i = 0; i < bins; i++){numcdf_vec[i] /= sum;} -// for(unsigned int i = 0; i < bins; i++){ -// double x = start+double(i)*step; -// printf("------------------\n%i -> ",i); -// double cdfx = getcdf(x); -// double gt = 0.5 * erfc(-((x-mean)/(stdval+regularization)) * M_SQRT1_2); -// printf("gt: %f est: %f err: %f\n",gt,cdfx,cdfx-gt); -// } -//exit(0); -// for(unsigned int i = 0; i < bins; i++){ -// printf("%5.5i -> %5.5f -> %10.10f\n",i,start+double(i)*step,numcdf_vec[i]); -// } } From e393bc4d2b3901f2e2a644148f646c44a48bc0a7 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 20 Sep 2016 19:32:07 +0200 Subject: [PATCH 106/255] Fixed a bug --- .../benchmark/src/benchmark_visualization.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp b/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp index d447f78a..04f30240 100644 --- a/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp +++ b/dynamic_object_retrieval/benchmark/src/benchmark_visualization.cpp @@ -41,7 +41,7 @@ cv::Mat make_visualization_image(cv::Mat& query_image, const string& query_label { cv::Mat result_image; tie(result_image, individual_images) = make_image(clouds, T, sweep_paths, optional_text); - individual_images.insert(individual_images.begin(), result_image.clone()); + individual_images.insert(individual_images.begin(), query_image.clone()); return add_query_image(result_image, query_image, query_label); } @@ -267,7 +267,7 @@ pair > make_image(std::vector& results, co int height = 200; cv::Mat visualization = cv::Mat::zeros(height*sizes.first, width*sizes.second, CV_8UC3); - cv::Mat individual_images; + vector individual_images; int counter = 0; for (CloudT::Ptr& cloud : results) { From c5a812b14f51752058c8c4581f1bbdc12b0b3158 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 20 Sep 2016 22:56:25 +0200 Subject: [PATCH 107/255] cleaned some prints --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 27 ++++-------- .../metaroom_additional_view_processing.cpp | 42 ++++++------------- .../quasimodo_models/src/core/Camera.cpp | 15 +------ .../quasimodo_models/src/core/RGBDFrame.cpp | 4 +- .../src/modelupdater/ModelUpdater.cpp | 6 +-- .../src/weightfunctions/Distribution.cpp | 2 +- 6 files changed, 26 insertions(+), 70 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 77107133..4c52d427 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -145,7 +145,7 @@ Eigen::Matrix4d getMat(tf::StampedTransform tf){ reglib::Model * load_metaroom_model(std::string sweep_xml){ int slash_pos = sweep_xml.find_last_of("/"); std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; - printf("folder: %s\n",sweep_folder.c_str()); + printf("load_metaroom_model(%s)\n",sweep_folder.c_str()); SimpleXMLParser parser; SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); @@ -153,7 +153,6 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ reglib::Model * sweepmodel = 0; Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); - cout << m2 << endl << endl; std::vector current_room_frames; for (size_t i=0; ify = aCameraModel.fy(); cam->cx = aCameraModel.cx(); cam->cy = aCameraModel.cy(); - cam->print(); + //cam->print(); Eigen::Matrix4d m = m2*getMat(roomData.vIntermediateRoomCloudTransformsRegistered[i]); reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, m); @@ -185,7 +184,6 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ } sweepmodel->recomputeModelPoints(); - printf("sweep nr points: %i\n",sweepmodel->points.size()); return sweepmodel; } @@ -207,33 +205,30 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec massregmod->nomask = true; massregmod->stopval = 0.0001; -printf("%s::%i\n",__FILE__,__LINE__); - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); mu->occlusion_penalty = 15; mu->massreg_timeout = 60*4; mu->viewer = viewer; -printf("%s::%i\n",__FILE__,__LINE__); + if(models.size() > 0 && bg->frames.size() > 0){ std::vector cpmod; -printf("%s::%i\n",__FILE__,__LINE__); + //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); bg->recomputeModelPoints(); -printf("%s::%i\n",__FILE__,__LINE__); + cpmod.push_back(Eigen::Matrix4d::Identity());//,models.front()->relativeposes.front().inverse() * bg->relativeposes.front()); massregmod->addModel(bg); printf("bg->points = %i\n",bg->points.size()); -printf("%s::%i\n",__FILE__,__LINE__); + for(int j = 0; j < models.size(); j++){ - printf("%s::%i\n",__FILE__,__LINE__); + //printf("%s::%i\n",__FILE__,__LINE__); //models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); models[j]->recomputeModelPoints(); cpmod.push_back(bg->frames.front()->pose.inverse() * models[j]->frames.front()->pose);//bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); massregmod->addModel(models[j]); } -printf("%s::%i\n",__FILE__,__LINE__); reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); for(int j = 0; j < models.size(); j++){ @@ -247,10 +242,8 @@ printf("%s::%i\n",__FILE__,__LINE__); models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; } } - printf("%s::%i\n",__FILE__,__LINE__); }else if(models.size() > 1){ std::vector cpmod; -printf("%s::%i\n",__FILE__,__LINE__); Eigen::Matrix4d first = Eigen::Matrix4d::Identity(); //Eigen::Matrix4d first = mod_po_vec.front().front(); for(int j = 0; j < models.size(); j++){ @@ -259,10 +252,8 @@ printf("%s::%i\n",__FILE__,__LINE__); cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); massregmod->addModel(models[j]); } -printf("%s::%i\n",__FILE__,__LINE__); reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); for(int j = 0; j < models.size(); j++){ - printf("%s::%i\n",__FILE__,__LINE__); Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ @@ -273,9 +264,7 @@ printf("%s::%i\n",__FILE__,__LINE__); models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; } } - printf("%s::%i\n",__FILE__,__LINE__); } -printf("%s::%i\n",__FILE__,__LINE__); delete massregmod; std::vector bgcp; @@ -286,7 +275,6 @@ printf("%s::%i\n",__FILE__,__LINE__); bgcf.push_back(bg->frames[k]); bgmask.push_back(bg->modelmasks[k]->getMask()); } -printf("%s::%i\n",__FILE__,__LINE__); for(int j = 0; j < models.size(); j++){ reglib::Model * model = models[j]; @@ -300,7 +288,6 @@ printf("%s::%i\n",__FILE__,__LINE__); for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} masks.push_back(mask); } -printf("%s::%i\n",__FILE__,__LINE__); std::vector movemask; std::vector dynmask; mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index ee505e81..4d58ce84 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -662,45 +662,39 @@ void processMetaroom(std::string path){ std::vector< reglib::Model * > models; models.push_back(fullmodel); -printf("%s::%i\n",__FILE__,__LINE__); + std::vector< std::vector< cv::Mat > > internal; std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; -printf("%s::%i\n",__FILE__,__LINE__); + quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization_lvl > 0); -printf("%s::%i\n",__FILE__,__LINE__); bg->fullDelete(); -printf("%s::%i\n",__FILE__,__LINE__); delete bg; -printf("%s::%i\n",__FILE__,__LINE__); + remove_old_seg(sweep_folder); -printf("%s::%i\n",__FILE__,__LINE__); + for(unsigned int i = 0; i < models.size(); i++){ -printf("%s::%i\n",__FILE__,__LINE__); std::vector internal_masks = internal[i]; std::vector external_masks = external[i]; std::vector dynamic_masks = dynamic[i]; -printf("%s::%i\n",__FILE__,__LINE__); reglib::Model * model = models[i]; -printf("%s::%i\n",__FILE__,__LINE__); std::vector mod_po; std::vector mod_fr; std::vector mod_mm; model->getData(mod_po, mod_fr, mod_mm); -printf("%s::%i\n",__FILE__,__LINE__); + std::vector dynamic_frameid; std::vector dynamic_pixelid; -printf("%s::%i\n",__FILE__,__LINE__); + std::vector moving_frameid; std::vector moving_pixelid; -printf("%s::%i\n",__FILE__,__LINE__); + pcl::PointCloud::Ptr dynamiccloud (new pcl::PointCloud); pcl::PointCloud::Ptr movingcloud (new pcl::PointCloud); pcl::PointCloud::Ptr cloud (new pcl::PointCloud); -printf("%s::%i\n",__FILE__,__LINE__); + for(unsigned int j = 0; j < mod_fr.size(); j++){ -printf("%s::%i\n",__FILE__,__LINE__); reglib::RGBDFrame * frame = mod_fr[j]; Eigen::Matrix4d p = mod_po[j]; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); @@ -764,20 +758,19 @@ printf("%s::%i\n",__FILE__,__LINE__); } } } -printf("%s::%i\n",__FILE__,__LINE__); + // if(visualization_lvl > 0 && cloud->points.size() > 0){ // viewer->removeAllPointClouds(); // viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); // viewer->spin(); // } -printf("%s::%i\n",__FILE__,__LINE__); + printf("dynamiccloud: %i\n",int(dynamiccloud->points.size())); if(dynamiccloud->points.size() > 0){ - printf("%s::%i\n",__FILE__,__LINE__); // Creating the KdTree object for the search method of the extraction pcl::search::KdTree::Ptr dynamictree (new pcl::search::KdTree); dynamictree->setInputCloud (dynamiccloud); -printf("%s::%i\n",__FILE__,__LINE__); + std::vector dynamic_indices; pcl::EuclideanClusterExtraction dynamic_ec; dynamic_ec.setClusterTolerance (0.02); // 2cm @@ -786,7 +779,7 @@ printf("%s::%i\n",__FILE__,__LINE__); dynamic_ec.setSearchMethod (dynamictree); dynamic_ec.setInputCloud (dynamiccloud); dynamic_ec.extract (dynamic_indices); -printf("%s::%i\n",__FILE__,__LINE__); + for (unsigned int d = 0; d < dynamic_indices.size(); d++){ std::vector< std::vector > inds; inds.resize(mod_fr.size()); @@ -797,13 +790,11 @@ printf("%s::%i\n",__FILE__,__LINE__); inds[dynamic_frameid[pid]].push_back(dynamic_pixelid[pid]); cloud_cluster->points.push_back(dynamiccloud->points[pid]); } -printf("%s::%i\n",__FILE__,__LINE__); char buf [1024]; sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),d); QFile file(buf); if (file.exists()){file.remove();} if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); @@ -902,29 +893,20 @@ printf("%s::%i\n",__FILE__,__LINE__); delete xmlWriter; } } - printf("%s::%i\n",__FILE__,__LINE__); } - printf("%s::%i\n",__FILE__,__LINE__); } - printf("%s::%i\n",__FILE__,__LINE__); - fullmodel->fullDelete(); - printf("%s::%i\n",__FILE__,__LINE__); delete fullmodel; - printf("%s::%i\n",__FILE__,__LINE__); // delete bgmassreg; delete reg; - printf("%s::%i\n",__FILE__,__LINE__); delete mu; - printf("%s::%i\n",__FILE__,__LINE__); printf("publishing file %s to %s\n",path.c_str(),outtopic.c_str()); std_msgs::String msg; msg.data = path; out_pub.publish(msg); ros::spinOnce(); - printf("%s::%i\n",__FILE__,__LINE__); } void chatterCallback(const std_msgs::String::ConstPtr& msg){ diff --git a/quasimodo/quasimodo_models/src/core/Camera.cpp b/quasimodo/quasimodo_models/src/core/Camera.cpp index 64b3d974..b6b88974 100644 --- a/quasimodo/quasimodo_models/src/core/Camera.cpp +++ b/quasimodo/quasimodo_models/src/core/Camera.cpp @@ -3,7 +3,6 @@ namespace reglib { - unsigned int camera_id_count = 0; Camera::Camera(){ @@ -19,21 +18,9 @@ Camera::Camera(){ pixel_weights = 0; pixel_sums = 0; - -// bias = 500; -// pixel_weights = new double[width*height]; -// pixel_sums = new double[width*height]; -// for(unsigned int i = 0; i < width*height; i++){ -// pixel_weights[i] = bias; -// pixel_sums[i] = bias; -// } - } -Camera::~Camera(){ -// delete[] pixel_weights; -// delete[] pixel_sums; -} +Camera::~Camera(){} void Camera::save(std::string path){ printf("save camera to %s\n",path.c_str()); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index d871c60b..b2dade11 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -460,7 +460,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt DistanceWeightFunction2PPR3 * funcZ = new DistanceWeightFunction2PPR3(); funcZ->zeromean = true; funcZ->startreg = 0.001; - funcZ->debugg_print = true; + funcZ->debugg_print = false; funcZ->bidir = false; funcZ->maxp = 0.999999; funcZ->maxd = 0.1; @@ -2248,7 +2248,7 @@ void RGBDFrame::save(std::string path){ } RGBDFrame * RGBDFrame::load(Camera * cam, std::string path){ - printf("RGBDFrame * RGBDFrame::load(Camera * cam, std::string path)\n"); + //printf("RGBDFrame * RGBDFrame::load(Camera * cam, std::string path)\n"); std::streampos size; char * buffer; diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index adc6cb67..804eda9f 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2029,7 +2029,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; tot_nr_pixels += nr_pixels; } - printf("tot_nr_pixels: %i\n",tot_nr_pixels); + //printf("tot_nr_pixels: %i\n",tot_nr_pixels); //Graph... std::vector< std::vector > interframe_connectionId; @@ -2076,7 +2076,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dfuncTMP->zeromean = false; //dfuncTMP->startreg = 0.0005; dfuncTMP->startreg = 0.0; - dfuncTMP->debugg_print = true; + dfuncTMP->debugg_print = false; dfuncTMP->bidir = true; //dfuncTMP->bidir = false; dfuncTMP->maxp = 0.9; @@ -2099,7 +2099,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s DistanceWeightFunction2PPR3 * nfuncTMP = new DistanceWeightFunction2PPR3(ggdnfunc); nfunc = nfuncTMP; nfuncTMP->startreg = 0.00; - nfuncTMP->debugg_print = true; + nfuncTMP->debugg_print = false; nfuncTMP->bidir = false; nfuncTMP->zeromean = true; nfuncTMP->maxp = 0.999; diff --git a/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp index aead55d1..dbcc2569 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp @@ -10,7 +10,7 @@ void Distribution::update(){ double Distribution::getval(double x){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} double Distribution::getcdf(double x){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} void Distribution::print(){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} -void Distribution::setRegularization(double x){printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__); regularization = x;update();} +void Distribution::setRegularization(double x){regularization = x;update();} double Distribution::getNoise(){return 1;} void Distribution::setNoise(double x){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} void Distribution::getMaxdMind(double & maxd, double & mind, double prob){ From cf5edb2c8a9ddaa427b52af75c71b811eb7b52df Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 20 Sep 2016 23:29:28 +0200 Subject: [PATCH 108/255] adding and removing prints --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 3 +++ .../quasimodo_models/src/modelupdater/ModelUpdater.cpp | 8 +++++++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 4c52d427..23c67a46 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -189,6 +189,7 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ } void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ + printf("running segment method\n"); boost::shared_ptr viewer; if(debugg){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); @@ -230,6 +231,7 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec massregmod->addModel(models[j]); } + printf("registering new data to background\n"); reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); for(int j = 0; j < models.size(); j++){ Eigen::Matrix4d change = mfrmod.poses[j+1];// * cpmod[j+1].inverse(); @@ -290,6 +292,7 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec } std::vector movemask; std::vector dynmask; + printf("computeMovingDynamicStatic\n"); mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions external.push_back(movemask); internal.push_back(masks); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 804eda9f..7f12e047 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2047,6 +2047,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s DistanceWeightFunction2 * dfunc; DistanceWeightFunction2 * nfunc; + printf("computing all residuals\n"); for(unsigned int i = 0; i < frames.size(); i++){ for(unsigned int j = 0; j < frames.size(); j++){ if(i == j){continue;} @@ -2063,6 +2064,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s + printf("training ppr\n"); double dstdval = 0; for(unsigned int i = 0; i < dvec.size(); i++){ dstdval += dvec[i]*dvec[i]; @@ -2121,6 +2123,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + printf("adding constraints\n"); for(unsigned int i = 0; i < frames.size(); i++){ int offset = offsets[i]; RGBDFrame * frame = frames[i]; @@ -2251,6 +2254,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s delete dfuncTMP; delete nfuncTMP; + printf("running inference\n"); + long interframeConnections = 0; for(unsigned int i = 0; i < interframe_connectionId.size();i++){ for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ @@ -2541,8 +2546,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s cloud_cluster2->points.push_back(p); } - printf("score: %f avg: %f\n",score,score/totsum); if(score > 200){ + printf("score: %f avg: %f\n",score,score/totsum); + printf("dynamic group found\n"); for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ labels[dynamicdata[dynamic_indices[d].indices[ind]]] = 3; } From 9f2c63e653b3cfc2244ec1a1d762ad66fe6a6323 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 23 Sep 2016 17:06:37 +0200 Subject: [PATCH 109/255] ... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 66 +- .../metaroom_additional_view_processing.cpp | 9 +- quasimodo/quasimodo_models/CMakeLists.txt | 2 +- .../quasimodo_models/include/core/RGBDFrame.h | 3 + .../include/core/ReprojectionResult.h | 1 + .../include/modelupdater/ModelUpdater.h | 188 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 2018 +++++------------ .../src/core/ReprojectionResult.cpp | 3 + .../quasimodo_models/src/model/Model.cpp | 4 +- .../src/modelupdater/ModelUpdater.cpp | 347 ++- 10 files changed, 917 insertions(+), 1724 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 23c67a46..879743b8 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -189,6 +189,7 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ } void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ + double startTime = getTime(); printf("running segment method\n"); boost::shared_ptr viewer; if(debugg){ @@ -211,46 +212,35 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec mu->occlusion_penalty = 15; mu->massreg_timeout = 60*4; mu->viewer = viewer; - + printf("total segment part1 time: %5.5fs\n",getTime()-startTime); if(models.size() > 0 && bg->frames.size() > 0){ std::vector cpmod; + if(bg->points.size() == 0){bg->recomputeModelPoints();} - //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); - bg->recomputeModelPoints(); - - cpmod.push_back(Eigen::Matrix4d::Identity());//,models.front()->relativeposes.front().inverse() * bg->relativeposes.front()); + cpmod.push_back(Eigen::Matrix4d::Identity()); massregmod->addModel(bg); - printf("bg->points = %i\n",bg->points.size()); for(int j = 0; j < models.size(); j++){ - //printf("%s::%i\n",__FILE__,__LINE__); - //models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - models[j]->recomputeModelPoints(); + if(models[j]->points.size() == 0){models[j]->recomputeModelPoints();} cpmod.push_back(bg->frames.front()->pose.inverse() * models[j]->frames.front()->pose);//bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); massregmod->addModel(models[j]); } - - printf("registering new data to background\n"); + printf("total segment part1b time: %5.5fs\n",getTime()-startTime); reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); for(int j = 0; j < models.size(); j++){ - Eigen::Matrix4d change = mfrmod.poses[j+1];// * cpmod[j+1].inverse(); - + Eigen::Matrix4d change = mfrmod.poses[j+1]; for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; } - for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; } } }else if(models.size() > 1){ std::vector cpmod; - Eigen::Matrix4d first = Eigen::Matrix4d::Identity(); - //Eigen::Matrix4d first = mod_po_vec.front().front(); for(int j = 0; j < models.size(); j++){ - //models[j]->points = mu->getSuperPoints(models[j]->relativeposes,models[j]->frames,models[j]->modelmasks,1,false); - models[j]->recomputeModelPoints(); + if(models[j]->points.size() == 0){models[j]->recomputeModelPoints();} cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); massregmod->addModel(models[j]); } @@ -269,6 +259,8 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec } delete massregmod; + printf("total segment part2 time: %5.5fs\n",getTime()-startTime); + std::vector bgcp; std::vector bgcf; std::vector bgmask; @@ -280,27 +272,31 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec for(int j = 0; j < models.size(); j++){ reglib::Model * model = models[j]; - std::vector masks; - for(unsigned int i = 0; i < model->frames.size(); i++){ - reglib::RGBDFrame * frame = model->frames[i]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} - masks.push_back(mask); - } - std::vector movemask; - std::vector dynmask; - printf("computeMovingDynamicStatic\n"); - mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions - external.push_back(movemask); - internal.push_back(masks); - dynamic.push_back(dynmask); +// std::vector masks; +// for(unsigned int i = 0; i < model->frames.size(); i++){ +// reglib::RGBDFrame * frame = model->frames[i]; +// reglib::Camera * cam = frame->camera; +// cv::Mat mask; +// mask.create(cam->height,cam->width,CV_8UC1); +// unsigned char * maskdata = (unsigned char *)(mask.data); +// for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} +// masks.push_back(mask); +// } +// std::vector movemask; +// std::vector dynmask; +// printf("computeMovingDynamicStatic\n"); +// mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions +// external.push_back(movemask); +// internal.push_back(masks); +// dynamic.push_back(dynmask); + + SegmentationResults sr = mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions } delete reg; delete mu; + printf("total segment time: %5.5fs\n",getTime()-startTime); + exit(0); } } diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 4d58ce84..1bb85d53 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -529,13 +529,16 @@ reglib::Model * processAV(std::string path){ fullmodel->modelmasks.push_back(masks[i]); fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); } + fullmodel->recomputeModelPoints(); + }else{ + fullmodel->points = sweep->points; } delete reg; delete mu; delete sweep; - fullmodel->recomputeModelPoints(); + return fullmodel; } @@ -634,7 +637,7 @@ void processMetaroom(std::string path){ std::vector mm; fullmodel->getData(po, fr, mm); //fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); - fullmodel->recomputeModelPoints(); + //fullmodel->recomputeModelPoints(); SimpleXMLParser parser; SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); @@ -658,7 +661,7 @@ void processMetaroom(std::string path){ reglib::Model * bg = quasimodo_brain::load_metaroom_model(prev); //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); - bg->recomputeModelPoints(); + //bg->recomputeModelPoints(); std::vector< reglib::Model * > models; models.push_back(fullmodel); diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 77159046..30b72a65 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -3,7 +3,7 @@ project(quasimodo_models) #find_package(catkin REQUIRED)# quasimodo_msgs)# COMPONENTS quasimodo_msgs) #set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") -set(CMAKE_CXX_FLAGS "-O2 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") +set(CMAKE_CXX_FLAGS "-O2 -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") find_package(catkin REQUIRED COMPONENTS #metaroom_xml_parser diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 40a1de69..41e6a0df 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -42,6 +42,9 @@ namespace reglib cv::Mat depth; cv::Mat normals; cv::Mat depthedges; + + cv::Mat de; + cv::Mat ce; int * labels; int nr_labels; diff --git a/quasimodo/quasimodo_models/include/core/ReprojectionResult.h b/quasimodo/quasimodo_models/include/core/ReprojectionResult.h index 1507ff3a..ac799218 100644 --- a/quasimodo/quasimodo_models/include/core/ReprojectionResult.h +++ b/quasimodo/quasimodo_models/include/core/ReprojectionResult.h @@ -22,6 +22,7 @@ class ReprojectionResult{ ReprojectionResult(); ReprojectionResult(unsigned long si, unsigned long di, double rz, double a, double rr, double rg, double rb, double nz, double nrgb); ~ReprojectionResult(); + void print(); }; } diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 0a952270..2d405025 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -33,95 +33,115 @@ #include #include #include +#include //#include "../../densecrf/include/densecrf.h" namespace reglib { - using namespace std; - using namespace Eigen; - using namespace cv; - - class UpdatedModels{ - public: - std::vector< Model * > new_models; - std::vector< Model * > updated_models; - std::vector< Model * > unchanged_models; - std::vector< Model * > deleted_models; - - UpdatedModels(){} - }; - - class ModelUpdater{ - public: - double occlusion_penalty; - double massreg_timeout; - boost::shared_ptr viewer; - Model * model; - Mesh * mesher; - - int show_init_lvl;//init show - int show_refine_lvl;//refine show - bool show_scoring;//fuse scoring show - - ModelUpdater(); - ModelUpdater(Model * model_); - ~ModelUpdater(); - - - virtual FusionResults registerModel(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1); - virtual void fuse(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1); - virtual UpdatedModels fuseData(FusionResults * f, Model * model1,Model * model2); - - virtual bool isRefinementNeeded(); - virtual bool refineIfNeeded(); - - virtual void makeInitialSetup(); - - virtual double getCompareUtility(Matrix4d p, RGBDFrame* frame, ModelMask* mask, vector & cp, vector & cf, vector & cm); - virtual void getGoodCompareFrames(vector & cp, vector & cf, vector & cm); - - virtual pcl::PointCloud::Ptr getPCLnormalcloud(vector & points); - virtual void addSuperPoints(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int type = 1, bool debugg = false); - virtual vector getSuperPoints(vector cp, vector cf, vector cm, int type = 1, bool debugg = false); - - virtual void getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2); - - virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); - virtual void getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); - virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength,double debugg = false); - virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks); - virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg = false); - - virtual void computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); - - virtual OcclusionScore computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1, bool debugg = false); - virtual OcclusionScore computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp = Matrix4d::Identity(), int step = 1, bool debugg = false); - virtual OcclusionScore computeOcclusionScore(Model * model1, Model * model2, Matrix4d rp = Matrix4d::Identity(),int step = 1, bool debugg = false); - virtual vector > computeOcclusionScore(vector models, vector rps, int step = 1, bool debugg = false); - - virtual double computeOcclusionScoreCosts(vector models); - - virtual void addModelsToVector(vector & models, vector & rps, Model * model, Matrix4d rp); - - virtual void refine(double reg = 0.05,bool useFullMask = false, int visualization = 0); - virtual void show(bool stop = true); - virtual void pruneOcclusions(); -// virtual OcclusionScore computeOcclusionScore(RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step = 1, bool debugg = false); - virtual OcclusionScore computeOcclusionScore(RGBDFrame * src, ModelMask * src_modelmask, RGBDFrame * dst, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step = 1, bool debugg = false); - virtual void computeResiduals(std::vector & residuals, std::vector & weights, RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, Eigen::Matrix4d p, bool debugg = false); - - virtual std::vector > computeAllOcclusionScores( RGBDFrame * src, cv::Mat src_mask, RGBDFrame * dst, cv::Mat dst_mask,Eigen::Matrix4d p, bool debugg = false); - virtual void computeMassRegistration(std::vector current_poses, std::vector current_frames,std::vector current_masks); - - std::vector > getOcclusionScores(std::vector current_poses, std::vector current_frames, std::vector current_modelmasks, bool debugg_scores = false, double speedup = 1.0); - std::vector > getScores(std::vector > occlusionScores); - std::vector getPartition(std::vector< std::vector< float > > & scores, int dims = 2, int nr_todo = 5, double timelimit = 2); - - virtual void recomputeScores(); - - CloudData * getCD(std::vector current_poses, std::vector current_frames,std::vector current_masks, int step); - }; +using namespace std; +using namespace Eigen; +using namespace cv; + +class UpdatedModels{ +public: + std::vector< Model * > new_models; + std::vector< Model * > updated_models; + std::vector< Model * > unchanged_models; + std::vector< Model * > deleted_models; + + UpdatedModels(){} + ~UpdatedModels(){} +}; + +class SegmentationResults{ +public: + + std::vector< std::vector< unsigned long > > component_dynamic; + std::vector< double > scores_dynamic; + std::vector< double > total_dynamic; + std::vector< std::vector< unsigned long > > component_moving; + std::vector< double > scores_moving; + std::vector< double > total_moving; + + SegmentationResults(){} + ~SegmentationResults(){} +}; + +class ModelUpdater{ +public: + double occlusion_penalty; + double massreg_timeout; + boost::shared_ptr viewer; + Model * model; + Mesh * mesher; + + std::map benchtime; + + int show_init_lvl;//init show + int show_refine_lvl;//refine show + bool show_scoring;//fuse scoring show + + ModelUpdater(); + ModelUpdater(Model * model_); + ~ModelUpdater(); + + + virtual FusionResults registerModel(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1); + virtual void fuse(Model * model2, Eigen::Matrix4d guess = Eigen::Matrix4d::Identity(), double uncertanity = -1); + virtual UpdatedModels fuseData(FusionResults * f, Model * model1,Model * model2); + + virtual bool isRefinementNeeded(); + virtual bool refineIfNeeded(); + + virtual void makeInitialSetup(); + + virtual double getCompareUtility(Matrix4d p, RGBDFrame* frame, ModelMask* mask, vector & cp, vector & cf, vector & cm); + virtual void getGoodCompareFrames(vector & cp, vector & cf, vector & cm); + + virtual pcl::PointCloud::Ptr getPCLnormalcloud(vector & points); + virtual void addSuperPoints(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int type = 1, bool debugg = false); + virtual vector getSuperPoints(vector cp, vector cf, vector cm, int type = 1, bool debugg = false); + + virtual void getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2); + + virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); + virtual void getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); + virtual void getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, std::vector & framesp1_test, std::vector & framesp1, double * overlaps, double * occlusions, RGBDFrame* frame2, std::vector & framesp2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); + virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength,double debugg = false); + virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks); + virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg = false); + + //virtual void computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); + virtual SegmentationResults computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); + + virtual OcclusionScore computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1, bool debugg = false); + virtual OcclusionScore computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp = Matrix4d::Identity(), int step = 1, bool debugg = false); + virtual OcclusionScore computeOcclusionScore(Model * model1, Model * model2, Matrix4d rp = Matrix4d::Identity(),int step = 1, bool debugg = false); + virtual vector > computeOcclusionScore(vector models, vector rps, int step = 1, bool debugg = false); + + virtual double computeOcclusionScoreCosts(vector models); + + virtual void addModelsToVector(vector & models, vector & rps, Model * model, Matrix4d rp); + + virtual void refine(double reg = 0.05,bool useFullMask = false, int visualization = 0); + virtual void show(bool stop = true); + virtual void pruneOcclusions(); + // virtual OcclusionScore computeOcclusionScore(RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step = 1, bool debugg = false); + virtual OcclusionScore computeOcclusionScore(RGBDFrame * src, ModelMask * src_modelmask, RGBDFrame * dst, ModelMask * dst_modelmask, Eigen::Matrix4d p, int step = 1, bool debugg = false); + virtual void computeResiduals(std::vector & residuals, std::vector & weights, RGBDFrame * src, cv::Mat src_mask, ModelMask * src_modelmask, RGBDFrame * dst, cv::Mat dst_mask, ModelMask * dst_modelmask, Eigen::Matrix4d p, bool debugg = false); + + virtual std::vector > computeAllOcclusionScores( RGBDFrame * src, cv::Mat src_mask, RGBDFrame * dst, cv::Mat dst_mask,Eigen::Matrix4d p, bool debugg = false); + virtual void computeMassRegistration(std::vector current_poses, std::vector current_frames,std::vector current_masks); + + std::vector > getOcclusionScores(std::vector current_poses, std::vector current_frames, std::vector current_modelmasks, bool debugg_scores = false, double speedup = 1.0); + std::vector > getScores(std::vector > occlusionScores); + std::vector getPartition(std::vector< std::vector< float > > & scores, int dims = 2, int nr_todo = 5, double timelimit = 2); + + virtual void recomputeScores(); + + CloudData * getCD(std::vector current_poses, std::vector current_frames,std::vector current_masks, int step); +}; } diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index b2dade11..bf24c520 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -293,16 +293,334 @@ RGBDFrame * RGBDFrame::clone(){ return new RGBDFrame(camera->clone(), rgb.clone(),depth.clone(),capturetime, pose, true); } +//std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ +// cv::Mat src = frame->rgb.clone(); +// unsigned short * depthdata = (unsigned short *)(frame->depth.data); +// const float idepth = frame->camera->idepth_scale; + +// cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); + +// unsigned char * srcdata = (unsigned char *)src.data; +// unsigned int width = src.cols; +// unsigned int height = src.rows; + +// std::vector dxc; +// dxc.resize(width*height); +// for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0;} + +// std::vector dyc; +// dyc.resize(width*height); +// for(unsigned int i = 0; i < width*height;i++){dyc[i] = 0;} + +// std::vector src_dxdata; +// src_dxdata.resize(width*height); +// for(unsigned int i = 0; i < width*height;i++){src_dxdata[i] = 0;} + +// std::vector src_dydata; +// src_dydata.resize(width*height); +// for(unsigned int i = 0; i < width*height;i++){src_dydata[i] = 0;} + +// std::vector maxima_dxdata; +// maxima_dxdata.resize(width*height); +// for(unsigned int i = 0; i < width*height;i++){maxima_dxdata[i] = 0;} + +// std::vector maxima_dydata; +// maxima_dydata.resize(width*height); +// for(unsigned int i = 0; i < width*height;i++){maxima_dydata[i] = 0;} + + +// for(unsigned int w = 0; w < width;w++){ +// for(unsigned int h = 0; h < height;h++){ +// int ind = h*width+w; +// src_dxdata[ind] = 0; +// src_dydata[ind] = 0; +// } +// } + +// unsigned int chans = 3; +// for(unsigned int c = 0; c < chans;c++){ +// for(unsigned int w = 1; w < width;w++){ +// for(unsigned int h = 1; h < height;h++){ +// int ind = h*width+w; +// int dir = 1; +// src_dxdata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-1)+c]) / 255.0)/3.0; + +// dir = width; +// src_dydata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-width)+c]) / 255.0)/3.0; +// } +// } + +// } + +// for(unsigned int w = 1; w < width-1;w++){ +// for(unsigned int h = 1; h < height-1;h++){ +// int ind = h*width+w; +// maxima_dxdata[ind] = (src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] > src_dxdata[ind+1]); +// maxima_dydata[ind] = (src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] > src_dydata[ind+width]); +// } +// } + +// std::vector< std::vector > probs; +// for(unsigned int c = 0; c < chans;c++){ +// std::vector Xvec; +// int dir; +// for(unsigned int w = 1; w < width-1;w++){ +// for(unsigned int h = 1; h < height-1;h++){ +// int ind = h*width+w; +// dir = 1; +// Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + +// dir = width; +// Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); +// } +// } + +// Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); +// for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + +// double stdval = 0; +// for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} +// stdval = sqrt(stdval/double(Xvec.size())); + +// DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); +// func->zeromean = true; +// func->maxp = 0.9999; +// func->startreg = 0.5; +// func->debugg_print = false; +// //func->bidir = true; +// func->maxd = 256.0; +// func->histogram_size = 256; +// func->fixed_histogram_size = true; +// func->startmaxd = func->maxd; +// func->starthistogram_size = func->histogram_size; +// func->blurval = 0.005; +// func->stdval2 = stdval; +// func->maxnoise = stdval; +// func->reset(); +// func->computeModel(X); + +// std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} +// std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} +// for(unsigned int w = 1; w < width;w++){ +// for(unsigned int h = 1; h < height-1;h++){ +// int ind = h*width+w; + +// dir = 1; +// dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); + +// dir = width; +// dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); +// } +// } +// delete func; + +// probs.push_back(dx); +// probs.push_back(dy); +// } + +// { +// std::vector Xvec; +// for(unsigned int w = 1; w < width-1;w++){ +// for(unsigned int h = 1; h < height-1;h++){ +// int ind = h*width+w; +// float z = idepth*float(depthdata[ind]); + +// if(w > 1){ +// int dir = -1; +// int other2 = ind+2*dir; +// int other = ind+dir; + +// float z3 = idepth*float(depthdata[other2]); +// float z2 = idepth*float(depthdata[other]); +// z2 = 2*z2-z3; + +// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} +// } + +// if(h > 1){ +// int dir = -width; +// int other2 = ind+2*dir; +// int other = ind+dir; + +// float z3 = idepth*float(depthdata[other2]); +// float z2 = idepth*float(depthdata[other]); +// z2 = 2*z2-z3; + +// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} +// } +// } +// } + +// Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); +// for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + +// double stdval = 0; +// for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} +// stdval = sqrt(stdval/double(Xvec.size())); + +// DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); +// funcZ->zeromean = true; +// funcZ->startreg = 0.002; +// funcZ->debugg_print = false; +// //funcZ->bidir = true; +// funcZ->maxp = 0.999999; +// funcZ->maxd = 0.1; +// funcZ->histogram_size = 100; +// funcZ->fixed_histogram_size = true; +// funcZ->startmaxd = funcZ->maxd; +// funcZ->starthistogram_size = funcZ->histogram_size; +// funcZ->blurval = 0.5; +// funcZ->stdval2 = stdval; +// funcZ->maxnoise = stdval; +// funcZ->reset(); +// funcZ->computeModel(X); + +// for(unsigned int w = 1; w < width-1;w++){ +// for(unsigned int h = 1; h < height-1;h++){ +// int ind = h*width+w; +// float z = idepth*float(depthdata[ind]); + +// if(w > 1){ +// int dir = -1; +// int other2 = ind+2*dir; +// int other = ind+dir; + +// float z3 = idepth*float(depthdata[other2]); +// float z2 = idepth*float(depthdata[other]); +// z2 = 2*z2-z3; + +// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} +// } + +// if(h > 1){ +// int dir = -width; +// int other2 = ind+2*dir; +// int other = ind+dir; + +// float z3 = idepth*float(depthdata[other2]); +// float z2 = idepth*float(depthdata[other]); +// z2 = 2*z2-z3; + +// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} +// } +// } +// } + +// std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} +// std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} + +// for(unsigned int w = 1; w < width-1;w++){ +// for(unsigned int h = 1; h < height-1;h++){ +// int ind = h*width+w; +// float z = idepth*float(depthdata[ind]); + +// if(w > 1){ +// int dir = -1; +// int other2 = ind+2*dir; +// int other = ind+dir; + + +// float z3 = idepth*float(depthdata[other2]); +// float z2 = idepth*float(depthdata[other]); + +// float dz = fabs(z-z2); +// if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} +// if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} +// } + +// if(h > 1){ +// int dir = -width; + +// int other2 = ind+2*dir; +// int other = ind+dir; + +// float z3 = idepth*float(depthdata[other2]); +// float z2 = idepth*float(depthdata[other]); + +// float dz = fabs(z-z2); +// if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} +// if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} +// } +// } +// } + +// delete funcZ; +// probs.push_back(dx); +// probs.push_back(dy); +// } + + +// for(unsigned int w = 0; w < width;w++){ +// for(unsigned int h = 0; h < height;h++){ +// int ind = h*width+w; + +// float probX = 0; +// float probY = 0; + +// if(w > 0 && w < width-1){ +// float ax = 0.5; +// float bx = 0.5; +// for(unsigned int p = 0; p < probs.size()-2; p+=2){ +// float pr = probs[p][ind]; +// ax *= pr; +// bx *= 1.0-pr; +// } +// float px = ax/(ax+bx); +// float current = 0; +// if(!frame->det_dilate.data[ind]){ current = (1-px) * float(maxima_dxdata[ind]);} +// else{ current = std::max(float(1-probs[probs.size()-2][ind]),0.8f*(1-px) * float(maxima_dxdata[ind]));} +// probX = 1-current; +// } + +// if(h > 0 && h < height-1){ +// float ay = 0.5; +// float by = 0.5; +// for(unsigned int p = 1; p < probs.size()-2; p+=2){ +// float pr = probs[p][ind]; +// ay *= pr; +// by *= 1.0-pr; +// } +// float py = ay/(ay+by); +// float current = 0; +// if(!frame->det_dilate.data[ind]){ current = (1-py) * float(maxima_dydata[ind]);} +// else{ current = std::max(float(1-probs[probs.size()-1][ind]),0.8f*(1-py) * float(maxima_dydata[ind]));} +// probY = 1-current; +// } + +// dxc[ind] = std::min(probX,probY); +// dyc[ind] = std::min(probX,probY); +// } +// } + + +// std::vector< std::vector > probs2; +// probs2.push_back(dxc); +// probs2.push_back(dyc); + +// cv::Mat edges; +// edges.create(height,width,CV_32FC3); +// float * edgesdata = (float *)edges.data; + +// for(unsigned int i = 0; i < width*height;i++){ +// edgesdata[3*i+0] = 0; +// edgesdata[3*i+1] = dxc[i]; +// edgesdata[3*i+2] = dyc[i]; +// } + +// // cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); +// // cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); +// // cv::waitKey(0); + +// return probs2; +//} + RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals){ -// GeneralizedGaussianDistribution * ggd = new GeneralizedGaussianDistribution(); -// ggd->update_numcdf_vec(); -// printf("update_numcdf_vec done\n"); -// for(double x = -4; x <= 4; x+=0.1){ -// ggd->getcdf(x); -// } -// printf("cdf loop done\n"); -// exit(0); - keyval = ""; + bool verbose = false; + if(verbose) + printf("------------------------------\n"); + double startTime = getTime(); + double completeStartTime = getTime(); + keyval = ""; sweepid = -1; id = RGBDFrame_id_counter++; @@ -315,21 +633,14 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt IplImage iplimg = rgb_; IplImage* img = &iplimg; - //printf("%s LINE:%i\n",__FILE__,__LINE__); - int width = img->width; - int height = img->height; - int sz = height*width; + int height = img->height; const double idepth = camera->idepth_scale; const double cx = camera->cx; const double cy = camera->cy; const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy; - // - // printf("camera: cx %5.5f cy %5.5f",cx,cy); - // printf(": fx %5.5f fy %5.5f",camera->fx,camera->fy); - // printf(": ifx %5.5f ify %5.5f\n",ifx,ify); connections.resize(1); connections[0].resize(1); @@ -343,81 +654,19 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt labels = new int[width*height]; for(int i = 0; i < width*height; i++){labels[i] = 0;} - - //printf("%s LINE:%i\n",__FILE__,__LINE__); unsigned short * depthdata = (unsigned short *)depth.data; unsigned char * rgbdata = (unsigned char *)rgb.data; -// rgbdata = 0; + if(verbose) + printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); -// rgbdata = new float[3*width*height]; -// for(int i = 0; i < 3*width*height; i++){ -// rgbdata[i] = float(img_rgbdata[i]); -// } - - - //std::vector< std::vector > probs = getImageProbs(this,5); - - cv::Mat de; de.create(height,width,CV_32FC3); float * dedata = (float*)de.data; - for(int i = 0; i < 3*width*height; i++){ - dedata[i] = 0; - } + for(int i = 0; i < 3*width*height; i++){dedata[i] = 0;} - cv::Mat ce; - ce.create(height,width,CV_32FC3); - float * cedata = (float*)ce.data; - for(int i = 0; i < 2*width*height; i++){ - cedata[i] = 0; - } std::vector Xvec; -// for(unsigned int w = 1; w < width-1;w++){ -// for(unsigned int h = 1; h < height-1;h++){ -// int ind = h*width+w; -// float z = idepth*float(depthdata[ind]); - -// if(w > 1){ -// int dir = -1; -// int other2 = ind+2*dir; -// int other = ind+dir; - -// float z3 = idepth*float(depthdata[other2]); -// float z2 = idepth*float(depthdata[other]); -// z2 = 2*z2-z3; - -// double noise1 = z*z; -// double noise2 = z2*z2; -// double noise3 = sqrt(noise1*noise1+noise2*noise2); - -// if(z2 > 0 || z > 0){ -// dedata[3*ind+1] = fabs((z-z2)/noise3);//(z*z+z2*z2)); -// Xvec.push_back(dedata[3*ind+1]); -// } -// } - -// if(h > 1){ -// int dir = -width; -// int other2 = ind+2*dir; -// int other = ind+dir; - -// float z3 = idepth*float(depthdata[other2]); -// float z2 = idepth*float(depthdata[other]); -// z2 = 2*z2-z3; - -// double noise1 = z*z; -// double noise2 = z2*z2; -// double noise3 = sqrt(noise1*noise1+noise2*noise2); -// if(z2 > 0 || z > 0){ -// dedata[3*ind+2] = fabs((z-z2)/noise3);//(z*z+z2*z2)); -// Xvec.push_back(dedata[3*ind+2]); -// } -// } -// } -// } - int step = 1; for(unsigned int w = step; w < width-step;w++){ for(unsigned int h = step; h < height-step;h++){ @@ -448,7 +697,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } - + if(verbose) + printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} @@ -477,1111 +727,230 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt for(unsigned int w = 1; w < width-1;w++){ for(unsigned int h = 1; h < height-1;h++){ int ind = h*width+w; - if(w > 1){ - dedata[3*ind+1] = 1-funcZ->getProb(dedata[3*ind+1]); - } - - if(h > 1){ - dedata[3*ind+2] = 1-funcZ->getProb(dedata[3*ind+2]); - } + if(w > 1){dedata[3*ind+1] = 1-funcZ->getProb(dedata[3*ind+1]);} + if(h > 1){dedata[3*ind+2] = 1-funcZ->getProb(dedata[3*ind+2]);} } } - delete funcZ; + if(verbose) + printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); + int blursize = 5; + cv::Mat blur_rgb = rgb.clone(); + cv::GaussianBlur( blur_rgb, blur_rgb, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); + unsigned char * blurdata = (unsigned char *)blur_rgb.data; + cv::Mat re; + re.create(height,width,CV_32FC3); + float * redata = (float*)re.data; + for(int i = 0; i < 3*width*height; i++){redata[i] = 0;} - cv::Mat det; - det.create(height,width,CV_8UC1); - unsigned char * detdata = (unsigned char*)det.data; - for(int i = 0; i < width*height; i++){ - detdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5)); - } + cv::Mat ge; + ge.create(height,width,CV_32FC3); + float * gedata = (float*)ge.data; + for(int i = 0; i < 3*width*height; i++){gedata[i] = 0;} + cv::Mat be; + be.create(height,width,CV_32FC3); + float * bedata = (float*)be.data; + for(int i = 0; i < 3*width*height; i++){bedata[i] = 0;} -// cv::namedWindow( "det", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det); -// cv::waitKey(0); - - - int dilation_size = 4; - cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) ); -/* - cv::Mat det2; - det2.create(height,width,CV_8UC3); - unsigned char * detdata2 = (unsigned char*)det2.data; - for(int i = 0; i < width*height; i++){ - detdata2[3*i+0] = detdata2[3*i+1] = detdata2[3*i+2] = detdata[i]; - } - - int area = 5; - for(unsigned int w = area; w < width-area;w++){ - for(unsigned int h = area; h < height-area;h++){ - bool p11 = detdata[h*width+w-0] != 0; - if(!p11){continue;} + ce.create(height,width,CV_32FC3); + float * cedata = (float*)ce.data; + for(int i = 0; i < 3*width*height; i++){cedata[i] = 0;} - int sum = 0; - for(unsigned int x = w-area; x < w+area;x++){ - for(unsigned int y = h-area; y < h+area;y++){ - sum += detdata[y*width+x] != 0; - } - } + std::vector XvecR; + std::vector XvecG; + std::vector XvecB; + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + bedata[3*ind+1] = fabs(blurdata[3*ind+0] - blurdata[3*(ind-1)+0]); + XvecB.push_back(bedata[3*ind+1]); - if(sum <= area/2){ - cv::circle(det2, cv::Point(w,h), 5, cv::Scalar(0,255,0)); - } + gedata[3*ind+1] = fabs(blurdata[3*ind+1] - blurdata[3*(ind-1)+1]); + XvecG.push_back(gedata[3*ind+1]); + redata[3*ind+1] = fabs(blurdata[3*ind+2] - blurdata[3*(ind-1)+2]); + XvecR.push_back(redata[3*ind+1]); } } + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 1; h < height;h++){ + int ind = h*width+w; + bedata[3*ind+2] = fabs(blurdata[3*ind+0] - blurdata[3*(ind-width)+0]); + XvecB.push_back(bedata[3*ind+1]); + gedata[3*ind+2] = fabs(blurdata[3*ind+1] - blurdata[3*(ind-width)+1]); + XvecG.push_back(gedata[3*ind+1]); - cv::namedWindow( "de", cv::WINDOW_AUTOSIZE ); cv::imshow( "de", de); - cv::namedWindow( "det", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det); - cv::namedWindow( "det2", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det2); - cv::namedWindow( "det_dilate", cv::WINDOW_AUTOSIZE ); cv::imshow( "det_dilate", det_dilate); - cv::namedWindow( "ce", cv::WINDOW_AUTOSIZE ); cv::imshow( "ce", ce); - cv::waitKey(0); -*/ -/* -{ + redata[3*ind+2] = fabs(blurdata[3*ind+2] - blurdata[3*(ind-width)+2]); + XvecR.push_back(redata[3*ind+1]); + } + } + std::vector src_dxdata; + src_dxdata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){src_dxdata[i] = 0;} - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); + std::vector src_dydata; + src_dydata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){src_dydata[i] = 0;} - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; + std::vector maxima_dxdata; + maxima_dxdata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){maxima_dxdata[i] = 0;} - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; + std::vector maxima_dydata; + maxima_dydata.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){maxima_dydata[i] = 0;} - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} - } - - if(h > 1){ - int dir = -width; - int other2 = ind+2*dir; - int other = ind+dir; - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - z2 = 2*z2-z3; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + src_dxdata[ind] = 0; + src_dydata[ind] = 0; + } + } - if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} + unsigned int chans = 3; + for(unsigned int c = 0; c < chans;c++){ + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 1; h < height;h++){ + int ind = h*width+w; + src_dxdata[ind] += fabs(float(blurdata[chans*ind+c] - blurdata[chans*(ind-1) +c]) / 255.0)/3.0; + src_dydata[ind] += fabs(float(blurdata[chans*ind+c] - blurdata[chans*(ind-width)+c]) / 255.0)/3.0; } } } - std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} - std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} - for(unsigned int w = 1; w < width-1;w++){ for(unsigned int h = 1; h < height-1;h++){ int ind = h*width+w; - float z = idepth*float(depthdata[ind]); + maxima_dxdata[ind] = (src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] > src_dxdata[ind+1]); + maxima_dydata[ind] = (src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] > src_dydata[ind+width]); + } + } - if(w > 1){ - int dir = -1; - int other2 = ind+2*dir; - int other = ind+dir; + double stdvalR = 0; + for(unsigned int i = 0; i < XvecR.size();i++){stdvalR += XvecR[i]*XvecR[i];} + stdvalR = sqrt(stdvalR/double(XvecR.size())); + + double stdvalG = 0; + for(unsigned int i = 0; i < XvecG.size();i++){stdvalG += XvecG[i]*XvecG[i];} + stdvalG = sqrt(stdvalG/double(XvecG.size())); + + double stdvalB = 0; + for(unsigned int i = 0; i < XvecB.size();i++){stdvalB += XvecB[i]*XvecB[i];} + stdvalB = sqrt(stdvalB/double(XvecB.size())); + + if(verbose) + printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); + + DistanceWeightFunction2PPR2 * funcR = new DistanceWeightFunction2PPR2(); + DistanceWeightFunction2PPR2 * funcG = new DistanceWeightFunction2PPR2(); + DistanceWeightFunction2PPR2 * funcB = new DistanceWeightFunction2PPR2(); + funcR->zeromean = funcG->zeromean = funcB->zeromean = true; + funcR->maxp = funcG->maxp = funcB->maxp = 0.9999; + funcR->startreg = funcG->startreg = funcB->startreg = 0.5; + funcR->debugg_print = funcG->debugg_print = funcB->debugg_print = false; + funcR->maxd = funcG->maxd = funcB->maxd = 256.0; + funcR->histogram_size = funcG->histogram_size = funcB->histogram_size = 256; + funcR->fixed_histogram_size = funcG->fixed_histogram_size = funcB->fixed_histogram_size = true; + funcR->startmaxd = funcG->startmaxd = funcB->startmaxd = funcR->maxd; + funcR->starthistogram_size = funcG->starthistogram_size = funcB->starthistogram_size = funcR->histogram_size; + funcR->blurval = funcG->blurval = funcB->blurval = 0.005; + + funcR->stdval2 = stdvalR; + funcR->maxnoise = stdvalR; + funcG->stdval2 = stdvalG; + funcG->maxnoise = stdvalG; + funcB->stdval2 = stdvalB; + funcB->maxnoise = stdvalB; + + funcR->reset(); + ((DistanceWeightFunction2*)funcR)->computeModel(XvecR); + funcG->reset(); + ((DistanceWeightFunction2*)funcG)->computeModel(XvecG); + funcB->reset(); + ((DistanceWeightFunction2*)funcB)->computeModel(XvecB); - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - float dz = fabs(z-z2); - if(z3 > 0){dz = std::min(dz,float(fabs(z- (2*z2-z3))));} - if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + if(w > 1){ + double pr = funcR->getProb(redata[3*ind+1]); + double pg = funcG->getProb(gedata[3*ind+1]); + double pb = funcB->getProb(bedata[3*ind+1]); + double pc = pr*pg*pb/(pr*pg*pb+(1-pr)*(1-pg)*(1-pb)); + redata[3*ind+1] = 1-pr; + gedata[3*ind+1] = 1-pg; + bedata[3*ind+1] = 1-pb; + cedata[3*ind+1] = 1-pc; } if(h > 1){ - int dir = -width; - - int other2 = ind+2*dir; - int other = ind+dir; - - float z3 = idepth*float(depthdata[other2]); - float z2 = idepth*float(depthdata[other]); - - float dz = fabs(z-z2); - if(z3 > 0){dz = std::min(dz,float(fabs(z- (2*z2-z3))));} - if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} + double pr = funcR->getProb(redata[3*ind+2]); + double pg = funcG->getProb(gedata[3*ind+2]); + double pb = funcB->getProb(bedata[3*ind+2]); + double pc = pr*pg*pb/(pr*pg*pb+(1-pr)*(1-pg)*(1-pb)); + redata[3*ind+2] = 1-pr; + gedata[3*ind+2] = 1-pg; + bedata[3*ind+2] = 1-pb; + cedata[3*ind+2] = 1-pc; } } } + delete funcR; + delete funcG; + delete funcB; + if(verbose) + printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); + cv::Mat cenms; + cenms.create(height,width,CV_32FC3); + float * cenmsdata = (float*)cenms.data; for(int i = 0; i < width*height; i++){ - dedata[3*i+0] = 1-dx[i]; - dedata[3*i+1] = 1-dy[i]; - dedata[2*i+2] = 0; + cenmsdata[3*i+0] = 0; + cenmsdata[3*i+1] = cedata[3*i+1]*maxima_dxdata[i]; + cenmsdata[3*i+2] = cedata[3*i+2]*maxima_dydata[i]; } -} -*/ -// cv::namedWindow( "de", cv::WINDOW_AUTOSIZE ); cv::imshow( "de", de); -// cv::namedWindow( "ce", cv::WINDOW_AUTOSIZE ); cv::imshow( "ce", ce); -// cv::waitKey(0); + cv::Mat det; + det.create(height,width,CV_8UC1); + unsigned char * detdata = (unsigned char*)det.data; + for(int i = 0; i < width*height; i++){ + detdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5)); + } + // cv::namedWindow( "de", cv::WINDOW_AUTOSIZE ); cv::imshow( "de", de); + // cv::namedWindow( "re", cv::WINDOW_AUTOSIZE ); cv::imshow( "re", re); + // cv::namedWindow( "ge", cv::WINDOW_AUTOSIZE ); cv::imshow( "ge", ge); + // cv::namedWindow( "be", cv::WINDOW_AUTOSIZE ); cv::imshow( "be", be); + // cv::namedWindow( "ce", cv::WINDOW_AUTOSIZE ); cv::imshow( "ce", ce); + // cv::namedWindow( "cenms", cv::WINDOW_AUTOSIZE ); cv::imshow( "cenms", cenms); + // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); + // cv::namedWindow( "blur", cv::WINDOW_AUTOSIZE ); cv::imshow( "blur", blur_rgb); + // //cv::namedWindow( "maximas", cv::WINDOW_AUTOSIZE ); cv::imshow( "blur", blur_rgb); + // cv::waitKey(0); + // cv::namedWindow( "det", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det); + // cv::waitKey(0); + int dilation_size = 4; + cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) ); + depthedges.create(height,width,CV_8UC1); unsigned char * depthedgesdata = (unsigned char *)depthedges.data; - for(int i = 0; i < width*height; i++){ - depthedgesdata[i] = 0; - } + for(int i = 0; i < width*height; i++){depthedgesdata[i] = 0;} - if(false){ - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - cloud->width = width; - cloud->height = height; - cloud->points.resize(width*height); - - boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0, 0, 0); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); - - float * zbase = new float[width*height]; - float * rbase = new float[width*height]; - float * gbase = new float[width*height]; - float * bbase = new float[width*height]; - - float * zest = new float[width*height]; - float * rest = new float[width*height]; - float * gest = new float[width*height]; - float * best = new float[width*height]; - - float * zdx = new float[width*height]; - float * rdx = new float[width*height]; - float * gdx = new float[width*height]; - float * bdx = new float[width*height]; - - float * zdy = new float[width*height]; - float * rdy = new float[width*height]; - float * gdy = new float[width*height]; - float * bdy = new float[width*height]; - - float * znext = new float[width*height]; - float * rnext = new float[width*height]; - float * gnext = new float[width*height]; - float * bnext = new float[width*height]; - - for(int i = 0; i < width*height; i++){ - zbase[i] = zest[i] = idepth*double(depthdata[i]); - bbase[i] = best[i] = float(rgbdata[3*i+0])/256.0; - gbase[i] = gest[i] = float(rgbdata[3*i+1])/256.0; - rbase[i] = rest[i] = float(rgbdata[3*i+2])/256.0; - zdx[i] = zdy[i] = 0; - rdx[i] = rdy[i] = 0; - gdx[i] = gdy[i] = 0; - bdx[i] = bdy[i] = 0; - } - - for(int it = 0; true; it++){ - if(it % 50 == 0){ - - - - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBA & p = cloud->points[ind]; - double z = zest[ind]; - if(z > 0){ - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - }else{ - p.x = 0; - p.y = 0; - p.z = 0; - } - p.b = best[ind]*256.0; - p.g = gest[ind]*256.0; - p.r = rest[ind]*256.0; - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - - //for(int w = 0; w < width; w++){ - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++) - { - //int h= height/2; - int ind = h*width+w; - pcl::PointXYZRGBA & p = cloud->points[ind]; - - double z1 = zest[ind]; - double x1 = (double(w) - cx) * z1 * ifx; - double y1 = (double(h) - cy) * z1 * ify; - Eigen::Vector3d point1 (x1,y1,z1); - - double z2 = zest[ind]+zdx[ind]; - double x2 = (double(w+1) - cx) * z2 * ifx; - double y2 = (double(h) - cy) * z2 * ify; - Eigen::Vector3d point2 (x2,y2,z2); - - double z3 = zest[ind]+zdy[ind]; - double x3 = (double(w) - cx) * z2 * ifx; - double y3 = (double(h+1) - cy) * z2 * ify; - Eigen::Vector3d point3 (x3,y3,z3); - - Eigen::Vector3d normal = (point2-point1).cross(point3-point1); - normal.normalize(); - - float r = 0.5*(normal(0)+1.0); - float g = 0.5*(normal(1)+1.0); - float b = 0.5*(normal(2)+1.0); - - p.b = b*255.0;//(2.0*normal(2)-1.0)*255.0; - p.g = g*255.0;//(2.0*normal(1)-1.0)*255.0; - p.r = r*255.0;//(2.0*normal(0)-1.0)*255.0; - -// printf("%4.4i -> x1: %3.3f %3.3f %3.3f x2: %3.3f %3.3f %3.3f normal: %3.3f %3.3f %3.3f shifted: %3.3f %3.3f %3.3f rgb: %3.3i %3.3i %3.3i\n",w,point2(0)-point1(0),point2(1)-point1(1),point2(2)-point1(2), point3(0)-point1(0),point3(1)-point1(1),point3(2)-point1(2), normal(0),normal(1),normal(2), 0.5*(normal(0)+1.0),0.5*(normal(1)+1.0),0.5*(normal(2)+1.0), p.r,p.g,p.b); - -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); -// viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud"); -// viewer->spin(); - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud"); - viewer->spin(); - - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBA & p = cloud->points[ind]; - double z = zest[ind]; - if(z > 0){ - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - }else{ - p.x = 0; - p.y = 0; - p.z = 0; - } - p.b = best[ind]*256.0; - p.g = gest[ind]*256.0; - p.r = rest[ind]*256.0; - } - } - } - - - float sumeL = 0; - float sumeR = 0; - float sumeU = 0; - float sumeD = 0; - - float sumeLpred = 0; - float sumeRpred = 0; - float sumeUpred = 0; - float sumeDpred = 0; - float angleVar = 100*pow(0.97,it); - printf("angleVar: %15.15f\n",angleVar); - - for(int w = 1; w < width-1; w++){ - for(int h = 1; h < height-1;h++){ - int iM = h*width+w; - int iL = iM-1; - int iR = iM+1; - int iU = iM-width; - int iD = iM+width; - - //Z - float zMbase = zbase[iM]; - float zM = zest[iM]; - float zL = zest[iL]; - float zR = zest[iR]; - float zU = zest[iU]; - float zD = zest[iD]; - - float zMdx = zdx[iM]; - float zLdx = zdx[iL]; - float zRdx = zdx[iR]; - float zUdx = zdx[iU]; - float zDdx = zdx[iD]; - - float zMdy = zdy[iM]; - float zLdy = zdy[iL]; - float zRdy = zdy[iR]; - float zUdy = zdy[iU]; - float zDdy = zdy[iD]; - - float zLpred = zL-zLdx; - float zRpred = zR+zRdx; - float zUpred = zU-zUdy; - float zDpred = zD+zDdy; - - //R - float rMbase = rbase[iM]; - float rM = rest[iM]; - float rL = rest[iL]; - float rR = rest[iR]; - float rU = rest[iU]; - float rD = rest[iD]; - - //G - float gMbase = gbase[iM]; - float gM = gest[iM]; - float gL = gest[iL]; - float gR = gest[iR]; - float gU = gest[iU]; - float gD = gest[iD]; - - //B - float bMbase = bbase[iM]; - float bM = best[iM]; - float bL = best[iL]; - float bR = best[iR]; - float bU = best[iU]; - float bD = best[iD]; - - //Diffs - - float dzL2 = (zM-zL)*(zM-zL)/(zM*zM*zM*zM+zL*zL*zL*zL); - float dzL2pred = (zM-zLpred)*(zM-zLpred)/(zM*zM*zM*zM+zL*zL*zL*zL); - - float drL = rM-rL; - float dgL = gM-gL; - float dbL = bM-bL; - - float dzR2 = (zM-zR)*(zM-zR)/(zM*zM*zM*zM+zR*zR*zR*zR); - float dzR2pred = (zM-zRpred)*(zM-zRpred)/(zM*zM*zM*zM+zR*zR*zR*zR); - float drR = rM-rR; - float dgR = gM-gR; - float dbR = bM-bR; - - float dzU2 = (zM-zU)*(zM-zU)/(zM*zM*zM*zM+zU*zU*zU*zU); - float dzU2pred = (zM-zUpred)*(zM-zUpred)/(zM*zM*zM*zM+zU*zU*zU*zU); - float drU = rM-rU; - float dgU = gM-gU; - float dbU = bM-bU; - - float dzD2 = (zM-zD)*(zM-zD)/(zM*zM*zM*zM+zD*zD*zD*zD); - float dzD2pred = (zM-zDpred)*(zM-zDpred)/(zM*zM*zM*zM+zD*zD*zD*zD); - float drD = rM-rD; - float dgD = gM-gD; - float dbD = bM-bD; - - int itthresh = 2000; - if(it < itthresh || dzL2 < dzL2pred){ - dzL2pred = dzL2; - zLpred = zL; - } - - if(it < itthresh || dzR2 < dzR2pred){ - dzR2pred = dzR2; - zRpred = zR; - } - - if(it < itthresh || dzU2 < dzU2pred){ - dzU2pred = dzU2; - zUpred = zU; - } - - if(it < itthresh || dzD2 < dzD2pred){ - dzD2pred = dzD2; - zDpred = zD; - } - - float zangleL = (zLdx-zMdx)*(zLdx-zMdx)+(zLdy-zMdy)*(zLdy-zMdy); - float zangleR = (zRdx-zMdx)*(zRdx-zMdx)+(zRdy-zMdy)*(zRdy-zMdy); - float zangleU = (zUdx-zMdx)*(zUdx-zMdx)+(zUdy-zMdy)*(zUdy-zMdy); - float zangleD = (zDdx-zMdx)*(zDdx-zMdx)+(zDdy-zMdy)*(zDdy-zMdy); - - - //Weights - float distVar = 0.01*0.01; - float colVar = 0.1*0.1; - - float diffZ = fabs(zM-zMbase)/(0.001*zMbase*zMbase); - - float weightM = pow(diffZ,2)*0.01*float(zMbase > 0);//exp(-0.5*(zM-zMbase)*(zM-zMbase)/distVar);//1.0*float(zMbase > 0); - float weightL = exp(-0.5*(dzL2/distVar + drL*drL/colVar + dgL*dgL/colVar + dbL*dbL/colVar));//*exp(-0.5*zangleL/angleVar); - float weightR = exp(-0.5*(dzR2/distVar + drR*drR/colVar + dgR*dgR/colVar + dbR*dbR/colVar));//*exp(-0.5*zangleR/angleVar); - float weightU = exp(-0.5*(dzU2/distVar + drU*drU/colVar + dgU*dgU/colVar + dbU*dbU/colVar));//*exp(-0.5*zangleU/angleVar); - float weightD = exp(-0.5*(dzD2/distVar + drD*drD/colVar + dgD*dgD/colVar + dbD*dbD/colVar));//*exp(-0.5*zangleD/angleVar); - - if((weightL > 0.4 && weightR < 0.2) || (weightL < 0.2 && weightR > 0.4) || (weightU > 0.4 && weightD < 0.2) || (weightU < 0.2 && weightD > 0.4)){ - pcl::PointXYZRGBA & p = cloud->points[iM]; - p.b = 0; - p.g = 255; - p.r = 0; - } - - - if(w == width/2 && h == height/2){ - printf("w: %i h:%i\n",w,h); - printf("M: %3.3f Base: %3.3f\n",zM,zMbase); - printf("L:: val: %3.3f dx: %3.3f dy: %3.3f pred: %3.3f\n",zL,zLdx,zLdy,zLpred); - printf("R:: val: %3.3f dx: %3.3f dy: %3.3f pred: %3.3f\n",zR,zRdx,zRdy,zRpred); - printf("U:: val: %3.3f dx: %3.3f dy: %3.3f pred: %3.3f\n",zU,zUdx,zUdy,zUpred); - printf("D:: val: %3.3f dx: %3.3f dy: %3.3f pred: %3.3f\n",zD,zDdx,zDdy,zDpred); - } - - float sumW = weightM+weightL+weightR+weightU+weightD; - if(sumW > 0){ - sumeL += dzL2*weightL; - sumeR += dzR2*weightR; - sumeU += dzU2*weightU; - sumeD += dzD2*weightD; - - sumeLpred += dzL2pred*weightL; - sumeRpred += dzR2pred*weightR; - sumeUpred += dzU2pred*weightU; - sumeDpred += dzD2pred*weightD; - - if(isnan(sumeLpred)){ - printf("w: %i h:%i\n",w,h); - printf("L:: val: %3.3f dx: %3.3f dy: %3.3f pred: %3.3f\n",zL,zLdx,zLdy,zLpred); - printf("normal sums: %10.10f %10.10f %10.10f %10.10f\npredic sums: %10.10f %10.10f %10.10f %10.10f\n",sumeL,sumeR,sumeU,sumeD,sumeLpred,sumeRpred,sumeUpred,sumeDpred); - exit(0); - } - - float znextM = (zM*weightM+zLpred*weightL+zRpred*weightR+zUpred*weightU+zDpred*weightD)/sumW; - znext[iM] = znextM; - if(weightL + weightR > 0){ - zdx[iM] = (weightL*(zL-znextM) + weightR*(znextM-zR))/(weightL + weightR); - } - - if(weightU + weightD > 0){ - zdy[iM] = (weightU*(zU-znextM) + weightD*(znextM-zD))/(weightU + weightD); - } - /* - if(fabs(znext[iM] - zMbase)/sqrt(zMbase*zMbase*zMbase*zMbase + znext[iM]*znext[iM]*znext[iM]*znext[iM]) > 0.01 && znext[iM] < 2.0){ - - printf("new value: %3.3f\n",znext[iM]); - printf("value M: %3.3f L %3.3f R %3.3f U %3.3f D %3.3f\n",zM,zL,zR,zU,zD); - printf("weight M: %3.3f L %3.3f R %3.3f U %3.3f D %3.3f\n",weightM,weightL,weightR,weightU,weightD); - - - pcl::PointXYZRGBA & p = cloud->points[iM]; - p.b = 0; - p.g = 255; - p.r = 0; - - viewer->removeAllShapes(); - viewer->addLine (cloud->points[iM],cloud->points[iL],(1-weightL),weightL,0,"L"); - viewer->addLine (cloud->points[iM],cloud->points[iR],(1-weightR),weightR,0,"R"); - viewer->addLine (cloud->points[iM],cloud->points[iU],(1-weightU),weightU,0,"U"); - viewer->addLine (cloud->points[iM],cloud->points[iD],(1-weightD),weightD,0,"D"); - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - - p.b = 255;//best[iM]*256.0; - p.g = 0;//gest[iM]*256.0; - p.r = 255;//rest[iM]*256.0; - - } - */ - }else{ - znext[iM] = zest[iM]; - } - - } - } - - printf("------\n"); - printf("normal sums: %10.10f %10.10f %10.10f %10.10f\npredic sums: %10.10f %10.10f %10.10f %10.10f\n",sumeL,sumeR,sumeU,sumeD,sumeLpred,sumeRpred,sumeUpred,sumeDpred); - -// std::memcpy(zest,znext,width*height*sizeof(float)); -// std::memcpy(rest,rnext,width*height*sizeof(float)); -// std::memcpy(gest,gnext,width*height*sizeof(float)); -// std::memcpy(best,bnext,width*height*sizeof(float)); - - float * ztmp = zest; zest = znext; znext = ztmp; -// float * rtmp = rest; rest = rnext; rnext = rtmp; -// float * gtmp = gest; gest = gnext; gnext = gtmp; -// float * btmp = best; best = bnext; bnext = btmp; - } - exit(0); - } - - if(false){ - - - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - cloud->width = width; - cloud->height = height; - cloud->points.resize(width*height); - - boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0, 0, 0); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); - // cv::Mat bilat; - // cv::adaptiveBilateralFilter(rgb, bilat, cv::Size(21,21), 30); - - // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); - // cv::namedWindow( "bilat", cv::WINDOW_AUTOSIZE ); cv::imshow( "bilat", bilat); - // cv::waitKey(0); - //Per channel - //Base - //est - //dx - //dy - //errX - //errY - std::vector channels_base; - std::vector channels_noise; - std::vector channels_est; - std::vector channels_dx; - std::vector channels_dy; - std::vector channels_errx; - std::vector channels_erry; - - cv::Mat z_base; - z_base.create(height,width,CV_32FC1); - float * zbase = (float *)z_base.data; - - for(int i = 0; i < width*height; i++){ - zbase[i] = idepth*double(depthdata[i]); - } - - channels_base.push_back(z_base); - for(int c = 0; c < 3; c++){ - cv::Mat current_base; - current_base.create(height,width,CV_32FC1); - float * currentbase = (float *)current_base.data; - - for(int i = 0; i < width*height; i++){ - currentbase[i] = float(rgbdata[3*i+c])/256.0; - } - channels_base.push_back(current_base); - } - - for(int c = 0; c < channels_base.size(); c++){ - float * data = (float *)(channels_base[c].data); - cv::Mat tmp; - tmp.create(height,width,CV_32FC1); - float * tmpdata = (float *)tmp.data; - for(int i = 0; i < width*height; i++){ - tmpdata[i] = data[i]; - } - channels_est.push_back(tmp); - } - - for(int c = 0; c < channels_base.size(); c++){ - cv::Mat tmp; - tmp.create(height,width,CV_32FC1); - float * tmpdata = (float *)tmp.data; - for(int i = 0; i < width*height; i++){ - tmpdata[i] = 0; - } - channels_dx.push_back(tmp); - } - - for(int c = 0; c < channels_base.size(); c++){ - cv::Mat tmp; - tmp.create(height,width,CV_32FC1); - float * tmpdata = (float *)tmp.data; - for(int i = 0; i < width*height; i++){ - tmpdata[i] = 0; - } - channels_dy.push_back(tmp); - } - - for(int c = 0; c < channels_base.size(); c++){ - cv::Mat tmp; - tmp.create(height,width,CV_32FC1); - float * tmpdata = (float *)tmp.data; - for(int i = 0; i < width*height; i++){ - tmpdata[i] = 0; - } - channels_errx.push_back(tmp); - } - - for(int c = 0; c < channels_base.size(); c++){ - cv::Mat tmp; - tmp.create(height,width,CV_32FC1); - float * tmpdata = (float *)tmp.data; - for(int i = 0; i < width*height; i++){ - tmpdata[i] = 0; - } - channels_erry.push_back(tmp); - } - - //Shared weights - cv::Mat wximg; - wximg.create(height,width,CV_32FC1); - float * wx = (float *)wximg.data; - for(int i = 0; i < width*height; i++){wx[i] = 1;} - - cv::Mat wyimg; - wyimg.create(height,width,CV_32FC1); - float * wy = (float *)wyimg.data; - for(int i = 0; i < width*height; i++){wy[i] = 1;} - - - - - for(int it = 0; true ; it++){ - - if(it % 1 == 0){ - for(int c = 0; c < channels_base.size(); c++){ - float * est = (float *)(channels_est[c].data); - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBA & p = cloud->points[ind]; - double z = est[ind]; - if(c == 0){ - if(z > 0){ - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - }else{ - p.x = 0; - p.y = 0; - p.z = 0; - } - } - if(c == 1){p.b = z*256.0;} - if(c == 2){p.g = z*256.0;} - if(c == 3){p.r = z*256.0;} - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - - viewer->removeAllShapes(); - for(int w0 = 0; w0 < width-1; w0++){ - for(int h0 = 0; h0 < height;h0+=4){ - int ind0 = h0*width+w0; - - int w1 = w0+1; - int h1 = h0; - int ind1 = h1*width+w1; - - pcl::PointXYZRGBA & p0 = cloud->points[ind0]; - pcl::PointXYZRGBA & p1 = cloud->points[ind1]; - if(p0.z == 0 || p1.z == 0){continue;} - - float weight = weightFunc(w0,h0,w1,h1,width, channels_est, channels_dx, channels_dy); - if(weight > 0.5){continue;} - char buf [1024]; - sprintf(buf,"line_%i_%i_%i_%i",w0,h0,w1,h1); - viewer->addLine (p0,p1,(1-weight),weight,0,buf); - } - } - - for(int w0 = 0; w0 < width; w0+=4){ - for(int h0 = 0; h0 < height-1;h0++){ - int ind0 = h0*width+w0; - - int w1 = w0; - int h1 = h0+1; - int ind1 = h1*width+w1; - - pcl::PointXYZRGBA & p0 = cloud->points[ind0]; - pcl::PointXYZRGBA & p1 = cloud->points[ind1]; - if(p0.z == 0 || p1.z == 0){continue;} - - float weight = weightFunc(w0,h0,w1,h1,width, channels_est, channels_dx, channels_dy); - - if(weight > 0.5){continue;} - char buf [1024]; - sprintf(buf,"line_%i_%i_%i_%i",w0,h0,w1,h1); - viewer->addLine (p0,p1,(1-weight),weight,0,buf); - } - } - viewer->spin(); - - //float pred(float targetW, float targetH, int sourceW, int sourceH, int width, float * est, float * dx, float * dy) - } - - // float mulbase = 0.1; - // for(int w = 1; w < width-1; w++){ - // for(int h = 1; h < height-1;h++){ - // int i = h*width+w; - // float weightBase = mulbase*((float*)(channels_base[0].data))[i] > 0; - // float weightL = weightFunc(w,h,w-1,h ,width, channels_est, channels_dx, channels_dy); - // float weightR = weightFunc(w,h,w+1,h ,width, channels_est, channels_dx, channels_dy); - // float weightU = weightFunc(w,h,w ,h-1,width, channels_est, channels_dx, channels_dy); - // float weightD = weightFunc(w,h,w ,h+1,width, channels_est, channels_dx, channels_dy); - // float sumW = weightBase+weightL+weightR+weightU+weightD; - // if(sumW == 0){continue;} - // for(int c = 0; c < channels_base.size(); c++){ - // float base = ((float*)(channels_base[c].data))[i]; - // float L = pred(w,h,w-1,h,width, (float*)(channels_est[c].data), (float*)(channels_dx[c].data), (float*)(channels_dy[c].data)); - // float R = pred(w,h,w+1,h,width, (float*)(channels_est[c].data), (float*)(channels_dx[c].data), (float*)(channels_dy[c].data)); - // float U = pred(w,h,w,h-1,width, (float*)(channels_est[c].data), (float*)(channels_dx[c].data), (float*)(channels_dy[c].data)); - // float D = pred(w,h,w,h+1,width, (float*)(channels_est[c].data), (float*)(channels_dx[c].data), (float*)(channels_dy[c].data)); - // float estimate = (weightBase*base+weightL*L+weightR*R+weightU*U+weightL*L+weightD*D)/sumW; - // ((float*)(channels_est[c].data))[i] = estimate; - // } - // } - // } - } - //} - - /* - //Update errors - for(int c = 0; c < channels_base.size(); c++){ - float * estdata = (float *)(channels_est[c].data); - - float * dxdata = (float *)(channels_dx[c].data); - float * dydata = (float *)(channels_dy[c].data); - - float * errxdata = (float *)(channels_errx[c].data); - float * errydata = (float *)(channels_erry[c].data); - for(int w = 0; w < width-1; w++){ - for(int h = 0; h < height;h++){ - int i = h*width+w; - int i2 = i+1;// cv::namedWindow( "wx", cv::WINDOW_AUTOSIZE ); cv::imshow( "wx", wximg); -// cv::namedWindow( "wy", cv::WINDOW_AUTOSIZE ); cv::imshow( "wy", wyimg); -// cv::waitKey(0); - float z0 = estdata[i]; - float z1 = estdata[i2]; - float slope0 = dxdata[i]; - float slope1 = dxdata[i2]; - - float e0 = (z0+slope0)-z1; - float e1 = (z1-slope1)-z0; - - errxdata[i] = fabs(e0)+fabs(e1); - } - } - - for(int w = 0; w < width; w++){ - for(int h = 0; h < height-1;h++){ - int i = h*width+w; - int i2 = i+width; - float z0 = estdata[i]; - float z1 = estdata[i2]; - float slope0 = dydata[i]; - float slope1 = dydata[i2]; - - float e0 = (z0+slope0)-z1; - float e1 = (z1-slope1)-z0; - - errydata[i] = fabs(e0)+fabs(e1); - } - } - } - - //Update weights - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int i = h*width+w; - float weight = 1; - for(int c = 0; c < channels_base.size(); c++){ - float e = ((float *)(channels_errx[c].data))[i]; - if(c == 0){weight *= exp(-0.5*e*e/0.1);} - if(c == 1){weight *= exp(-0.5*e*e/0.005);} - if(c == 2){weight *= exp(-0.5*e*e/0.005);} - if(c == 3){weight *= exp(-0.5*e*e/0.005);} - } - wx[i] = weight; - } - } - - //Update weights - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int i = h*width+w; - float weight = 1; - for(int c = 0; c < channels_base.size(); c++){ - float e = ((float *)(channels_erry[c].data))[i]; - if(c == 0){weight *= exp(-0.5*e*e/0.1);} - if(c == 1){weight *= exp(-0.5*e*e/0.005);} - if(c == 2){weight *= exp(-0.5*e*e/0.005);} - if(c == 3){weight *= exp(-0.5*e*e/0.005);} - } - wy[i] = weight; - } - } - -// cv::namedWindow( "wx", cv::WINDOW_AUTOSIZE ); cv::imshow( "wx", wximg); -// cv::namedWindow( "wy", cv::WINDOW_AUTOSIZE ); cv::imshow( "wy", wyimg); -// cv::waitKey(0); - - //Update points - double wbase = 0.1; - for(int c = 0; c < channels_base.size(); c++){ - float * base = (float *)(channels_base[c].data); - float * est = (float *)(channels_est[c].data); - float * dx = (float *)(channels_dx[c].data); - float * dy = (float *)(channels_dy[c].data); - for(int w = 1; w < width-1; w++){ - for(int h = 1; h < height-1;h++){ - int i = h*width+w; - - float wtmp = wbase * float(base[i] > 0); - if(wtmp == 0){continue;} - float w0 = wx[i-1] * float(base[i-1] > 0); - float w1 = wx[i] * float(base[i+1] > 0); - float w2 = wy[i-1] * float(base[i-width] > 0); - float w3 = wy[i] * float(base[i+width] > 0); - - float z = base[i]; - float pred0 = est[i-1] +dx[i-1]; - float pred1 = est[i+1] -dx[i+1]; - float pred2 = est[i-width] +dy[i-width]; - float pred3 = est[i+width] -dy[i+width]; - - double sumz = wtmp*z+w0*pred0+w1*pred1+w2*pred2+w3*pred3; - double sumw = wtmp+w0+w1+w2+w3; - - if(false && w % 40 == 0 && h % 40 == 0){ - printf("%4.4i %4.4i %4.4i -> M(%4.4f %4.4f) ",c,w,h,z,wbase); - printf("est: %4.4f %4.4f %4.4f ", est[i-1],est[i],est[i+1]); - printf("dx: %4.4f %4.4f %4.4f ", dx[i-1], dx[i], dx[i+1]); - printf("wx: %4.4f %4.4f", wx[i-1], wx[i]); - printf("\n"); - } - - //if(w % 40 == 0 && h % 40 == 0){printf("%i %i %i -> M(%4.4f %4.4f) L(%4.4f %4.4f) R(%4.4f %4.4f) U(%4.4f %4.4f) D(%4.4f %4.4f)\n",c,w,h,z,wbase,pred0,w0,pred1,w1,pred2,w2,pred3,w3);} - if(sumw > 0){ - est[i] = sumz/sumw; - }else{ - //est[i] = 0; - } - - } - } - */ - // exit(0); - // } - - // //Update slopes - - - // //} - - //// cv::namedWindow( "wx", cv::WINDOW_AUTOSIZE ); cv::imshow( "wx", wximg); - //// cv::namedWindow( "wy", cv::WINDOW_AUTOSIZE ); cv::imshow( "wy", wyimg); - //// cv::waitKey(0); - // } - - // cv::namedWindow( "est", cv::WINDOW_AUTOSIZE ); cv::imshow( "est", channels_est[c] ); - // cv::namedWindow( "dx", cv::WINDOW_AUTOSIZE ); cv::imshow( "dx", channels_dx[c] ); - // cv::namedWindow( "dy", cv::WINDOW_AUTOSIZE ); cv::imshow( "dy", channels_dy[c] ); - // cv::namedWindow( "errx", cv::WINDOW_AUTOSIZE ); cv::imshow( "errx", channels_errx[c]); - // cv::namedWindow( "erry", cv::WINDOW_AUTOSIZE ); cv::imshow( "erry", channels_erry[c]); - // cv::waitKey(0); - exit(0); - //Shared weights - - //Diffs - cv::Mat diffzimg; - diffzimg.create(height,width,CV_32FC2); - float * diffz = (float *)diffzimg.data; - - cv::Mat diffrimg; - diffrimg.create(height,width,CV_32FC2); - float * diffr = (float *)diffrimg.data; - - cv::Mat diffgimg; - diffgimg.create(height,width,CV_32FC2); - float * diffg = (float *)diffgimg.data; - - cv::Mat diffbimg; - diffbimg.create(height,width,CV_32FC2); - float * diffb = (float *)diffbimg.data; - - - - cv::Mat colimg_est; - colimg_est.create(height,width,CV_32FC3); - float * cest = (float *)colimg_est.data; - - cv::Mat colimg_base; - colimg_base.create(height,width,CV_32FC3); - float * cbase = (float *)colimg_base.data; - - cv::Mat cn_est; - cn_est.create(height,width,CV_32FC3); - float * cnest = (float *)cn_est.data; - - cv::Mat z_est; - z_est.create(height,width,CV_32FC3); - float * zest = (float *)z_est.data; - - - - cv::Mat nimg; - nimg.create(height,width,CV_32FC3); - float * nest = (float *)nimg.data; - - - cv::Mat dz_est; - dz_est.create(height,width,CV_32FC2); - float * dzest = (float *)dz_est.data; - - - - for(int i = 0; i < width*height; i++){ - zbase[i] = zest[i] = idepth*double(depthdata[i]); - } - - for(int i = 0; i < 3*width*height; i++){ - cbase[i] = cest[i] = float(rgbdata[i])/256.0; - } - - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int i = h*width+w; - dzest[2*i+0] = 0; - dzest[2*i+1] = 0; - } - } - - - - for(int it = 0; true; it++){ - printf("it: %i\n",it); - if(it % 100 == 0){ - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBA & p = cloud->points[ind]; - p.b = cest[3*ind+0]*256.0; - p.g = cest[3*ind+1]*256.0; - p.r = cest[3*ind+2]*256.0; - double z = zest[ind]; - if(z > 0){ - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - }else{ - p.x = 0; - p.y = 0; - p.z = 0; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - } - - //// Compute normals - // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", colimg_est ); - // cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); cv::imshow( "normals", nimg ); - // cv::namedWindow( "dz", cv::WINDOW_AUTOSIZE ); cv::imshow( "dz", dz_est ); - // cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", z_est); - // cv::waitKey(0); - - // for(int w = 0; w < width; w++){ - // for(int h = 0; h < height;h++){ - // int i = h*width+w; - // double z = zest[i]; - // if(w < width-1){ dzest[2*i+0] = z-zest[i+1];} - // else{ dzest[2*i+0] = dzest[2*i-2];} - - // if(h < height-1){ dzest[2*i+1] = z-zest[i+width];} - // else{ dzest[2*i+1] = dzest[2*(i-width)+1];} - // } - // } - - double w0 = 0.001; - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int i = h*width+w; - - if(w < 103 && w > 97 && h == 100){ - printf("mid %i: base: %5.5f %5.5f\n",w,zbase[i],zest[i]); - } - double z = zbase[i]; - double sumw = w0/(z*z); - double sumz = sumw*zbase[i]; - - if(z == 0){ - sumz = 0; - sumw = 0; - } - - if(w > 0){ - int i2 = i-1; - - double predz = zest[i2]+dzest[2*i+0]; - double weight = fabs(z -predz) < 0.05; - sumz+=weight*predz; - sumw+=weight; - - // if(w == 100 && h == 100){ - // printf("left %i: base: %5.5f %5.5f\n",w-1,zbase[i2],zest[i2]); - // } - } - - if(w < width-1){ - int i2 = i+1; - double predz = zest[i2]-dzest[2*i+0]; - double weight = fabs(z -predz) < 0.05; - sumz+=weight*predz; - sumw+=weight; - } - - if(h > 0){ - int i2 = i-width; - double predz = zest[i2]+dzest[2*i+0]; - double weight = fabs(z -predz) < 0.05; - sumz+=weight*predz; - sumw+=weight; - } - - if(h < height-1){ - int i2 = i+width; - double predz = zest[i2]-dzest[2*i+0]; - double weight = fabs(z -predz) < 0.05; - sumz+=weight*predz; - sumw+=weight; - } - - if(sumw == 0){ - zest[i] = 0; - }else{ - zest[i] = sumz/sumw; - } - } - } - - - - } - // double * z_base = new double[width*height]; - // double * col_base = new double[3*width*height]; - // double * z_est = new double[width*height]; - // double * col_est = new double[3*width*height]; - // double * normal_est = new double[3*width*height]; - - // for(int i = 0; i < width*height; i++){ - // z_base[i] = z_est[i] = idepth*double(depthdata[ind]); - // col_est[3*i+0] = rgbdata[3*i+0]; - // col_est[3*i+1] = rgbdata[3*i+1]; - // col_est[3*i+2] = rgbdata[3*i+2]; - // } - // delete[] z_base; - // delete[] col_base; - // delete[] z_est; - // delete[] col_est; - // delete[] normal_est; - } - - //printf("%s LINE:%i\n",__FILE__,__LINE__); + if(verbose) + printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); if(compute_normals){ normals.create(height,width,CV_32FC3); float * normalsdata = (float *)normals.data; @@ -1591,7 +960,6 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt cloud->width = width; cloud->height = height; cloud->points.resize(width*height); - //cloud->dense = false; for(int w = 0; w < width; w++){ for(int h = 0; h < height;h++){ @@ -1613,15 +981,9 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } - //printf("%s LINE:%i\n",__FILE__,__LINE__); - pcl::IntegralImageNormalEstimation ne; + pcl::IntegralImageNormalEstimation ne; ne.setInputCloud(cloud); - bool tune = false; - unsigned char * combidata; - cv::Mat combined; - - int NormalEstimationMethod = 0; int MaxDepthChangeFactor = 20; int NormalSmoothingSize = 7; int depth_dependent_smoothing = 1; @@ -1636,10 +998,6 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt int ind = h*width+w; pcl::PointXYZRGBA p = cloud->points[ind]; pcl::Normal p2 = normals_cloud->points[ind]; - - - //if(w % 20 == 0 && h % 20 == 0){printf("%i %i -> point(%f %f %f) normal(%f %f %f)\n",w,h,p.x,p.y,p.z,p2.normal_x,p2.normal_y,p2.normal_z);} - if(!isnan(p2.normal_x)){ normalsdata[3*ind+0] = p2.normal_x; normalsdata[3*ind+1] = p2.normal_y; @@ -1651,98 +1009,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } } - - // cv::Mat curvature; - // curvature.create(height,width,CV_8UC3); - // unsigned char * curvaturedata = (unsigned char *)curvature.data; - // for(int w = 0; w < width; w++){ - // for(int h = 0; h < height;h++){ - // int ind = h*width+w; - // pcl::Normal p2 = normals_cloud->points[ind]; - // //if(w % 5 == 0 && h % 5 == 0){printf("%i %i -> curvature %f\n",w,h,p2.curvature);} - // curvaturedata[3*ind+0] = 255.0*p2.curvature; - // curvaturedata[3*ind+1] = 255.0*p2.curvature; - // curvaturedata[3*ind+2] = 255.0*p2.curvature; - // } - // } - // cv::namedWindow( "curvature", cv::WINDOW_AUTOSIZE ); - // cv::imshow( "curvature", curvature ); - // cv::waitKey(0); - - - - //printf("%s LINE:%i\n",__FILE__,__LINE__); - if(tune){ - combined.create(height,2*width,CV_8UC3); - combidata = (unsigned char *)combined.data; - - cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); - cv::namedWindow( "combined", cv::WINDOW_AUTOSIZE ); - - //cv::createTrackbar( "NormalEstimationMethod", "combined", &NormalEstimationMethod, 3, on_trackbar ); - //cv::createTrackbar( "MaxDepthChangeFactor", "combined", &MaxDepthChangeFactor, 1000, on_trackbar ); - //cv::createTrackbar( "NormalSmoothingSize", "combined", &NormalSmoothingSize, 100, on_trackbar ); - //cv::createTrackbar( "depth_dependent_smoothing", "combined", &depth_dependent_smoothing, 1, on_trackbar ); - - //while(true){ - - if(NormalEstimationMethod == 0){ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);} - if(NormalEstimationMethod == 1){ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);} - if(NormalEstimationMethod == 2){ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);} - if(NormalEstimationMethod == 3){ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);} - - ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); - ne.setNormalSmoothingSize(NormalSmoothingSize); - ne.setDepthDependentSmoothing (depth_dependent_smoothing); - ne.compute(*normals_cloud); - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::Normal p2 = normals_cloud->points[ind]; - if(!isnan(p2.normal_x)){ - normalsdata[3*ind+0] = p2.normal_x; - normalsdata[3*ind+1] = p2.normal_y; - normalsdata[3*ind+2] = p2.normal_z; - }else{ - normalsdata[3*ind+0] = 2; - normalsdata[3*ind+1] = 2; - normalsdata[3*ind+2] = 2; - } - } - } - - - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - int indn = h*2*width+(w+width); - int indc = h*2*width+(w); - combidata[3*indc+0] = rgbdata[3*ind+0]; - combidata[3*indc+1] = rgbdata[3*ind+1]; - combidata[3*indc+2] = rgbdata[3*ind+2]; - pcl::Normal p2 = normals_cloud->points[ind]; - if(!isnan(p2.normal_x)){ - combidata[3*indn+0] = 255.0*fabs(p2.normal_x); - combidata[3*indn+1] = 255.0*fabs(p2.normal_y); - combidata[3*indn+2] = 255.0*fabs(p2.normal_z); - }else{ - combidata[3*indn+0] = 255; - combidata[3*indn+1] = 255; - combidata[3*indn+2] = 255; - } - } - } - char buf [1024]; - sprintf(buf,"combined%i.png",int(id)); - cv::imwrite( buf, combined ); - printf("saving: %s\n",buf); - cv::imshow( "combined", combined ); - cv::waitKey(0); - //while(!updated){cv::waitKey(50);} - updated = false; - //} - } - +if(false){ //pcl::PointCloud::Ptr cloud2 = getPCLcloud(); pcl::OrganizedEdgeFromRGBNormals oed; oed.setInputNormals (normals_cloud); @@ -1794,275 +1061,16 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt // out.data[3*ind+1] =255; // out.data[3*ind+2] =0; } - + } // cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); // cv::imshow( "edges", out ); // cv::waitKey(0); //show(true); } - - if(true){ - - for(int it = 0; it < 30; it++){ - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - } - } - } - } - - if(false){ - show(false); - - int * last = new int[width*height]; - for(int i = 0; i < width*height; i++){last[i] = 0;} - int current = 1; - - boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0, 0, 0); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); - - for(int w = 0; w < width; w+=5){ - for(int h = 0; h < height;h+=5){ - int ind = h*width+w; - - int edgetype = depthedgesdata[ind]; - if(edgetype != 0 && depthdata[ind] != 0){ - double xx = 0.001; - double xy = 0; - double xz = 0; - double yy = xx; - double yz = 0; - double zz = xx; - double sum = 0.001; - - double ww = 4.0; - double wh = 0; - double hh = 4.0; - double sum2 = 4.0; - - - double z = idepth*double(depthdata[ind]); - double x = (double(w) - cx) * z * ifx; - double y = (double(h) - cy) * z * ify; - - // for(int ww = 0; ww < width; ww++){ - // for(int hh = 0; hh < height;hh++){ - // int ind = hh*width+ww; - // pcl::PointXYZRGBA & p = cloud->points[ind]; - // p.b = 0; - // p.g = 255; - // p.r = 0; - // } - // } - - - - - std::vector todolistw; - todolistw.push_back(w); - std::vector todolisth; - todolisth.push_back(h); - current++; - last[ind] = current; - - cv::Mat rgbclone = rgb.clone(); - unsigned char * rgbclonedata = (unsigned char *)rgbclone.data; - - - cv::circle(rgbclone, cv::Point(w,h), 2, cv::Scalar(0,255,0)); - cv::imshow( "rgbclone", rgbclone ); - - - printf("ratio = ["); - double best_ratio = 0; - int tmp; - for(int go = 0; go < 100000 && go < todolistw.size(); go++){ - int cw = todolistw[go]; - int ch = todolisth[go]; - int ind = ch*width+cw; - - int dw = cw-w; - int dh = ch-h; - ww+=dw*dw; - wh+=dw*dh; - hh+=dh*dh; - sum2++; - - Eigen::Matrix2d mimg; - mimg(0,0) = ww/sum2; - mimg(0,1) = wh/sum2; - mimg(1,0) = wh/sum2; - mimg(1,1) = hh/sum2; - - Eigen::EigenSolver es(mimg); - double e1 = (es.eigenvalues())(0).real(); - double e2 = (es.eigenvalues())(1).real(); - double ratio = 1; - if(e1 > e2){ratio = e1/e2;} - if(e2 > e1){ratio = e2/e1;} - printf("%5.5f ",ratio); - - if(ratio < best_ratio){ - ww-=dw*dw; - wh-=dw*dh; - hh-=dh*dh; - sum2--; - //cv::circle(rgbclone, cv::Point(cw,ch), 2, cv::Scalar(255,0,255)); - continue; - } - - best_ratio = ratio; - // cout << "The mimg are:" << endl << mimg << endl; - // cout << "The eigenvalues of mimg are:" << endl << es.eigenvalues() << endl; - // printf("%f %f\n",e1,e2); - // cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl; - - // B2=(-cov[0][0]-cov[1][1])*(-cov[0][0]-cov[1][1]); - // c= (4*cov[0][0]*cov[1][1])-(4*cov[0][1]*cov[1][0]); - - - // sq= sqrt(B2-c); - - - // B=(cov[0][0])+(cov[1][1]); - - // r1=(B+sq)(.5); - // r2=(B-sq)(.5); - - - // printf("\nThe eigenvalues are %f and %f", r1, r2); - - - double z2 = idepth*double(depthdata[ind]); - if(z2 > 0){ - double x2 = (double(cw) - cx) * z2 * ifx; - double y2 = (double(ch) - cy) * z2 * ify; - - double dx = x-x2; - double dy = y-y2; - double dz = z-z2; - - xx += dx*dx; - xy += dx*dy; - xz += dx*dz; - - yy += dy*dy; - yz += dy*dz; - - zz += dz*dz; - sum++; - } - - //double z = idepth*double(depthdata[ind]); - rgbclonedata[3*ind+0] = 0; - rgbclonedata[3*ind+1] = 0; - rgbclonedata[3*ind+2] = 255; - - // cloud->points[ind].r = 255; - // cloud->points[ind].g = 0; - // cloud->points[ind].b = 0; - - int startw = std::max(int(0),cw-1); - int stopw = std::min(int(width-1),cw+1); - - int starth = std::max(int(0),ch-1); - int stoph = std::min(int(height-1),ch+1); - - for(int tw = startw; tw <= stopw; tw++){ - for(int th = starth; th <= stoph; th++){ - int ind2 = th*width+tw; - if(depthedgesdata[ind2] == edgetype && last[ind2] != current){ - todolistw.push_back(tw); - todolisth.push_back(th); - last[ind2] = current; - } - } - } - - } - - - printf("];\n"); - ww /= sum2; - wh /= sum2; - hh /= sum2; - Eigen::Matrix2d mimg; - mimg(0,0) = ww; - mimg(0,1) = wh; - mimg(1,0) = wh; - mimg(1,1) = hh; - - Eigen::EigenSolver es(mimg); - cout << "The mimg are:" << endl << mimg << endl; - cout << "The eigenvalues of mimg are:" << endl << es.eigenvalues() << endl; - cout << "The matrix of eigenvectors, V, is:" << endl << es.eigenvectors() << endl << endl; - - //cv::circle(rgbclone, cv::Point(w,h), 3, cv::Scalar(0,255,0)); - - - cv::imshow( "rgbclone", rgbclone ); - cv::waitKey(0); - /* - - pcl::PointCloud::Ptr cloud2 (new pcl::PointCloud); - cloud2->width = width; - cloud2->height = height; - cloud2->points.resize(width*height); - //cloud->dense = false; - - unsigned short * depthdata = (unsigned short *)depth.data; - unsigned char * rgbdata = (unsigned char *)rgb.data; - for(int w3 = 0; w3 < width; w3++){ - for(int h3 = 0; h3 < height;h3++){ - int ind3 = h3*width+w3; - pcl::PointXYZRGBA & p = cloud2->points[ind3]; - p.b = rgbdata[3*ind3+0];//255; - p.g = rgbdata[3*ind3+1];//255; - p.r = rgbdata[3*ind3+2];//255; - } - } - - for(int go = 0; go < todolistw.size(); go++){ - int cw = todolistw[go]; - int ch = todolisth[go]; - int ind3 = ch*width+cw; - - pcl::PointXYZRGBA & p = cloud2->points[ind3]; - p.b = 0; - p.g = 0; - p.r = 255; - } - - for(int w3 = 0; w3 < width; w3++){ - for(int h3 = 0; h3 < height;h3++){ - int ind3 = h3*width+w3; - pcl::PointXYZRGBA & p = cloud2->points[ind3]; - double z3 = idepth*double(depthdata[ind3]); - if(z3 > 0){ - p.x = (double(w3) - cx) * z3 * ifx; - p.y = (double(h3) - cy) * z3 * ify; - p.z = z3; - }else{ - p.x = 0; - p.y = 0; - p.z = 0; - } - } - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud2, pcl::visualization::PointCloudColorHandlerRGBField(cloud2), "cloud"); - viewer->spin(); - */ - } - } - } - } - + if(verbose) + printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); + printf("complete time to create RGBD image: %5.5fs\n",getTime()-completeStartTime); } RGBDFrame::~RGBDFrame(){ @@ -2381,7 +1389,7 @@ std::vector RGBDFrame::getSuperPoints(Eigen::Matrix4d cp, unsigned i const float ify = 1.0/camera->fy; std::vector ret; - ret.resize(width*height); + ret.resize((width/step)*(height/step)); unsigned long count = 0; for(unsigned long h = 0; h < height;h += step){ diff --git a/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp b/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp index f471705a..12102698 100644 --- a/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp +++ b/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp @@ -16,5 +16,8 @@ ReprojectionResult::ReprojectionResult(unsigned long si, unsigned long di, doubl } ReprojectionResult::~ReprojectionResult(){} +void ReprojectionResult::print(){ + printf("src_ind = %i, dst_ind = %i, angle = %f, residualZ = %f, residualR = %f, residualG = %f, residualB = %f\n",src_ind, dst_ind, angle, residualZ, residualR, residualG, residualB); +} } diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 6a0fadfd..16188c03 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -140,8 +140,10 @@ void Model::addAllSuperPoints(std::vector & spvec, Eigen::Matrix4d p } void Model::recomputeModelPoints(Eigen::Matrix4d pose, boost::shared_ptr viewer){ - points.clear(); + double startTime = getTime(); + points.clear(); addAllSuperPoints(points,pose,viewer); + printf("recomputeModelPoints time: %5.5fs\n",getTime()-startTime); } void Model::showHistory(boost::shared_ptr viewer){ diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 7f12e047..ca9db762 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1,22 +1,10 @@ #include "modelupdater/ModelUpdater.h" -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include -//#include #include - #include "opencv2/core/core.hpp" #include "opencv2/features2d/features2d.hpp" #include "opencv2/highgui/highgui.hpp" #include "opencv2/calib3d/calib3d.hpp" - #include "core/Util.h" @@ -212,8 +200,6 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M if(debugg){ - //cf->show(true); - pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); dcloud->points.resize(dst_width*dst_height); for(unsigned int dst_w = 0; dst_w < dst_width; dst_w++){ @@ -229,10 +215,6 @@ OcclusionScore ModelUpdater::computeOcclusionScore(vector & spvec, M dcloud->points[dst_ind].r = dst_rgbdata[3*dst_ind+2]; dcloud->points[dst_ind].g = dst_rgbdata[3*dst_ind+1]; dcloud->points[dst_ind].b = dst_rgbdata[3*dst_ind+0]; - if(dst_w % 10 == 0 && dst_h % 10 == 0){ - //printf("%i %i -> %4.4f %4.4f %4.4f\n",dst_w,dst_h,dst_normalsdata[3*dst_ind+0],dst_h,dst_normalsdata[3*dst_ind+1],dst_h,dst_normalsdata[3*dst_ind+2]); - } - //if(false && dst_maskdata[dst_ind] == 255){ if(dst_maskdata[dst_ind] == 255){ dcloud->points[dst_ind].r = 255; dcloud->points[dst_ind].g = 000; @@ -1628,11 +1610,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int b } } - - - std::vector< std::vector > probs; - for(unsigned int c = 0; c < chans;c++){ std::vector Xvec; int dir; @@ -1913,8 +1891,6 @@ std::vector doInference(std::vector & prior, std::vector< std::vect for(unsigned int j = 0; j < connectionId[i].size();j++){ double weight = connectionStrength[i][j]; g -> add_edge( i, connectionId[i][j], weight, weight ); - //try{g -> add_edge( i, connectionId[i][j], weight, weight );} - //catch(std::exception ex){printf("%i %i -> weight: %20.20f\n",i,connectionId[i],weight);exit(0);} } } @@ -1931,31 +1907,55 @@ std::vector doInference(std::vector & prior, std::vector< std::vect return labels; } -double normalCFD(double value) -{ - return 0.5 * erfc(-value * M_SQRT1_2); +void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ + std::vector framesp1_test = frame1->getSuperPoints(Eigen::Matrix4d::Identity(),10,false); + std::vector rr_vec_test = frame2->getReprojections(framesp1_test,p,0,false); + + double inlierratio = double(rr_vec_test.size())/double(framesp1_test.size()); + + if( inlierratio < 0.01 ){return ;} + std::vector framesp1 = frame1->getSuperPoints(); + std::vector framesp2 = frame2->getSuperPoints();//p); + + getDynamicWeights(store_distance,dvec,nvec,dfunc,nfunc,p,frame1,framesp1_test,framesp1,overlaps,occlusions,frame2,framesp2,offset1,offset2,interframe_connectionId, interframe_connectionStrength, debugg); } -void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ +void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, std::vector & tframesp1_test, std::vector & tframesp1, double * overlaps, double * occlusions, RGBDFrame* frame2, std::vector & tframesp2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ + double totalstartTime = getTime(); + + std::string currentString; + double startTime; + startTime = getTime(); unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); - std::vector framesp1_test = frame1->getSuperPoints(p,10,false); - std::vector rr_vec_test = frame2->getReprojections(framesp1_test,Eigen::Matrix4d::Identity(),0,false); - + std::vector & framesp1_test = tframesp1_test;//frame1->getSuperPoints(Eigen::Matrix4d::Identity(),10,false); + std::vector rr_vec_test = frame2->getReprojections(framesp1_test,p,0,false); double inlierratio = double(rr_vec_test.size())/double(framesp1_test.size()); - //printf("inlierratio: %f nr_points: %i nr rep: %i\n",inlierratio,framesp1_test.size(),rr_vec_test.size()); + + currentString = "part1"; + if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;} + benchtime[currentString] += getTime() - startTime; + startTime = getTime(); + if( inlierratio < 0.01 ){return ;} - std::vector framesp1 = frame1->getSuperPoints(p); - std::vector framesp2 = frame2->getSuperPoints();//p); - std::vector rr_vec = frame2->getReprojections(framesp1,Eigen::Matrix4d::Identity(),0,false); + std::vector & framesp1 = tframesp1;//frame1->getSuperPoints(); + std::vector & framesp2 = tframesp2;//frame2->getSuperPoints(); - inlierratio = double(rr_vec.size())/double(framesp1.size()); - //printf("inlierratio: %f nr_points: %i nr rep: %i\n",inlierratio,framesp1.size(),rr_vec.size()); + currentString = "part2"; + if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;} + benchtime[currentString] += getTime() - startTime; + startTime = getTime(); + std::vector rr_vec = frame2->getReprojections(framesp1,p,0,false); + inlierratio = double(rr_vec.size())/double(framesp1.size()); unsigned long nr_rr = rr_vec.size(); - //printf("getDynamicWeights nr_rr: %i\n",nr_rr); + + currentString = "part3"; + if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;} + benchtime[currentString] += getTime() - startTime; + startTime = getTime(); pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); @@ -2008,6 +2008,15 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & occlusions[src_ind] = std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); } } + currentString = "part4"; + if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;} + benchtime[currentString] += getTime() - startTime; + startTime = getTime(); + + currentString = "total_getDyn"; + if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;} + benchtime[currentString] += getTime() - totalstartTime; + startTime = getTime(); if(debugg && totsum > 0){ pcl::PointCloud::Ptr src_cloud = getPointCloudFromVector(framesp1,3,255,0,255); @@ -2019,7 +2028,12 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & } } -void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ + +SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ +//void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ + double computeMovingDynamicStatic_startTime = getTime(); + + SegmentationResults sr; int tot_nr_pixels = 0; std::vector offsets; @@ -2029,7 +2043,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; tot_nr_pixels += nr_pixels; } - //printf("tot_nr_pixels: %i\n",tot_nr_pixels); //Graph... std::vector< std::vector > interframe_connectionId; @@ -2047,32 +2060,43 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s DistanceWeightFunction2 * dfunc; DistanceWeightFunction2 * nfunc; - printf("computing all residuals\n"); + double startTime = getTime(); + + std::vector< std::vector > framesp_test; + std::vector< std::vector > framesp; for(unsigned int i = 0; i < frames.size(); i++){ + framesp_test.push_back(frames[i]->getSuperPoints(Eigen::Matrix4d::Identity(),10,false)); + framesp.push_back(frames[i]->getSuperPoints()); + } + printf("frames init time: %5.5fs\n",getTime()-startTime); + + startTime = getTime(); + std::vector< std::vector > bgsp; + for(unsigned int i = 0; i < bgcf.size(); i++){ + bgsp.push_back(bgcf[i]->getSuperPoints()); + } + printf("bg init time: %5.5fs\n",getTime()-startTime); + + startTime = getTime(); +// printf("computing all residuals\n"); + for(unsigned int i = 0; i < frames.size(); i++){ + std::vector & framesp1_test = framesp_test[i]; + std::vector & framesp1 = framesp[i]; for(unsigned int j = 0; j < frames.size(); j++){ if(i == j){continue;} Eigen::Matrix4d p = poses[i].inverse() * poses[j]; - getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i], 0, 0, frames[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); - } - - for(unsigned int j = 0; j < bgcf.size(); j++){ - //printf("%i , %i\n",i,j); - //Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; - //getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i], 0, 0, bgcf[j],-1,-1,interframe_connectionId,interframe_connectionStrength,debugg); + std::vector & framesp2 = framesp[j]; + getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp1_test,framesp1, 0, 0, frames[j],framesp2,offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); } } - - printf("training ppr\n"); +// printf("training ppr\n"); double dstdval = 0; - for(unsigned int i = 0; i < dvec.size(); i++){ - dstdval += dvec[i]*dvec[i]; - } + for(unsigned int i = 0; i < dvec.size(); i++){dstdval += dvec[i]*dvec[i];} dstdval = sqrt(dstdval/double(dvec.size()-1)); - - DistanceWeightFunction2PPR2 * dfuncTMP = new DistanceWeightFunction2PPR2(); + DistanceWeightFunction2PPR2 * dfuncTMP = new DistanceWeightFunction2PPR2(); dfunc = dfuncTMP; dfuncTMP->refine_mean = true; dfuncTMP->zeromean = false; @@ -2095,7 +2119,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dfuncTMP->reset(); dfunc->computeModel(dvec); - GeneralizedGaussianDistribution * ggdnfunc = new GeneralizedGaussianDistribution(true,true); ggdnfunc->nr_refineiters = 4; DistanceWeightFunction2PPR3 * nfuncTMP = new DistanceWeightFunction2PPR3(ggdnfunc); @@ -2117,17 +2140,26 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s nfuncTMP->reset(); nfunc->computeModel(nvec); -//exit(0); + printf("training time: %5.5fs\n",getTime()-startTime); + long frameConnections = 0; std::vector< std::vector< std::vector > > pixel_weights; pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - printf("adding constraints\n"); + double total_priortime = 0; + double total_connectiontime = 0; + double total_alloctime = 0; + double total_dealloctime = 0; + double total_Dynw = 0; + +// printf("adding constraints\n"); for(unsigned int i = 0; i < frames.size(); i++){ int offset = offsets[i]; RGBDFrame * frame = frames[i]; + startTime = getTime(); std::vector< std::vector > probs = getImageProbs(frame,9); + total_connectiontime += getTime()-startTime; pixel_weights.push_back(probs); unsigned short * depthdata = (unsigned short *)(frame->depth.data); @@ -2140,7 +2172,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s const float ifx = 1.0/camera->fx; const float ify = 1.0/camera->fy; - + startTime = getTime(); unsigned int nr_pixels = width * height; double * current_overlaps = new double[nr_pixels]; double * current_occlusions = new double[nr_pixels]; @@ -2149,26 +2181,29 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s current_occlusions[j] = 0; } - for(unsigned int j = 0; j < frames.size(); j++){ - if(i == j){continue;} - Eigen::Matrix4d p = poses[i].inverse() * poses[j]; - getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i], current_overlaps, current_occlusions, frames[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); - } - - double * bg_overlaps = new double[nr_pixels]; double * bg_occlusions = new double[nr_pixels]; for(unsigned int j = 0; j < nr_pixels; j++){ bg_overlaps[j] = 0; bg_occlusions[j] = 0; } + total_alloctime += getTime()-startTime; + + startTime = getTime(); + for(unsigned int j = 0; j < frames.size(); j++){ + if(i == j){continue;} + Eigen::Matrix4d p = poses[i].inverse() * poses[j]; + getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], current_overlaps, current_occlusions, frames[j],framesp[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); + } for(unsigned int j = 0; j < bgcf.size(); j++){ Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; - getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i], bg_overlaps, bg_occlusions, bgcf[j],-1,-1,interframe_connectionId,interframe_connectionStrength,false); + getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], bg_overlaps, bg_occlusions, bgcf[j],bgsp[j],-1,-1,interframe_connectionId,interframe_connectionStrength,false); } + total_Dynw += getTime()-startTime; + startTime = getTime(); unsigned char * detdata = (unsigned char*)(frame->det_dilate.data); float minprob = 0.01; for(unsigned int h = 0; h < height;h++){ @@ -2189,16 +2224,10 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s float bias = 0.0001; - - float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; - // float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover); - // float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover); - // float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover); - float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover) + minprob; float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover) + minprob; float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover) + 2.0*minprob; @@ -2207,16 +2236,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s priors[3*(offset+ind)+1] = p_dynamic_tot; priors[3*(offset+ind)+2] = p_static_tot; - // float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; - // float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; - // float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; - - - - // priors[3*(offset+ind)+0] = p_moving_tot; - // priors[3*(offset+ind)+1] = p_dynamic_tot; - // priors[3*(offset+ind)+2] = p_static_tot; - if(probs[0][ind] > 0.00000001){frameConnections++;} if(probs[1][ind] > 0.00000001){frameConnections++;} @@ -2244,17 +2263,27 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s cloud->points.push_back(point); } } + total_priortime += getTime()-startTime; + startTime = getTime(); delete[] current_occlusions; delete[] current_overlaps; delete[] bg_occlusions; delete[] bg_overlaps; + total_dealloctime += getTime()-startTime; } delete dfuncTMP; delete nfuncTMP; - printf("running inference\n"); + printf("total_priortime = %5.5fs\n", total_priortime); + printf("total_connectiontime = %5.5fs\n", total_connectiontime); + printf("total_alloctime = %5.5fs\n", total_alloctime); + printf("total_dealloctime = %5.5fs\n", total_dealloctime); + printf("total_Dynw = %5.5fs\n", total_Dynw); + + +// printf("running inference\n"); long interframeConnections = 0; for(unsigned int i = 0; i < interframe_connectionId.size();i++){ @@ -2263,9 +2292,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } - printf("frameConnections: %i\n",frameConnections); - printf("interframeConnections: %i\n",interframeConnections); - double start_inf = getTime(); gc::Graph * g = new gc::Graph(current_point,frameConnections+interframeConnections); for(unsigned long i = 0; i < current_point;i++){ @@ -2342,9 +2368,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s double end_inf = getTime(); printf("static inference time: %10.10fs\n",end_inf-start_inf); - - - printf("nr_dynamic: %i\n",nr_dynamic); long dynamic_frameConnections = 0; for(unsigned long i = 0; i < frames.size(); i++){ @@ -2380,8 +2403,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } - printf("dynamic_frameConnections: %i\n",dynamic_frameConnections); - printf("dynamic_interframeConnections: %i\n",dynamic_interframeConnections); start_inf = getTime(); gc::Graph * dynamic_g = new gc::Graph(nr_dynamic,dynamic_frameConnections+dynamic_interframeConnections); for(unsigned long i = 0; i < dyn_ind.size();i++){ @@ -2447,8 +2468,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } } - interframe_connectionId.clear(); - interframe_connectionStrength.clear(); dynamic_g -> maxflow(); @@ -2465,6 +2484,107 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } delete dynamic_g; + const unsigned int nr_frames = frames.size(); + const unsigned int width = frames[0]->camera->width; + const unsigned int height = frames[0]->camera->height; + const unsigned int pixels_per_image = width*height; + const unsigned int nr_pixels = nr_frames*pixels_per_image; + + std::vector< std::vector< unsigned long > > component_dynamic; + std::vector< double > scores_dynamic; + std::vector< double > total_dynamic; + std::vector< std::vector< unsigned long > > component_moving; + std::vector< double > scores_moving; + std::vector< double > total_moving; + + double probthresh = 0.5; + unsigned int number_of_dynamics = 0; + std::vector objectlabel; + objectlabel.resize(nr_pixels); + for(unsigned long i = 0; i < nr_pixels; i++){objectlabel[i] = 0;} + for(unsigned long ind = 0; ind < nr_pixels; ind++){ + if(valids[ind] && objectlabel[ind] == 0 && labels[ind] != 0){ + unsigned int current_label = labels[ind]; + number_of_dynamics++; + objectlabel[ind] = number_of_dynamics; + unsigned long todocounter = 0; + std::vector< unsigned long > todo; + todo.push_back(ind); + double score = 0; + double totsum = 0; + while(todocounter < todo.size()){ + unsigned long cind = todo[todocounter++]; + unsigned long frameind = cind / pixels_per_image; + + + unsigned long iind = cind % pixels_per_image; + unsigned long w = iind % width; + unsigned long h = iind / width; + + double p0 = priors[3*cind+0]; + double p1 = priors[3*cind+1]; + double p2 = priors[3*cind+2]; + + double s = 0; + if(p0 > p2){s += p1 - p0;} + else{ s += p1 - p2;} + score += s; + totsum++; + + float * dedata = (float*)(frames[frameind]->de.data); + + int dir; + dir = -1; + if(valids[cind+dir] && w > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + dir = 1; + if(valids[cind+dir] && w < (width-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + dir = -width; + if(valids[cind+dir] && h > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + dir = width; + if(valids[cind+dir] && h < (height-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + for(unsigned long j = 0; j < interframe_connectionId[cind].size();j++){ + unsigned long other = interframe_connectionId[cind][j]; + if(valids[other] && objectlabel[other] == 0 && labels[other] == current_label){ + objectlabel[other] = number_of_dynamics; + todo.push_back(other); + } + } + } + if(current_label == 1){ + printf("Dynamic: %f -> %f\n",score,totsum); + component_dynamic.push_back(todo); + scores_dynamic.push_back(score); + total_dynamic.push_back(totsum); + } + if(current_label == 2){ + printf("Moving: %f -> %f\n",score,totsum); + component_moving.push_back(todo); + scores_moving.push_back(score); + total_moving.push_back(totsum); + } + } + } + interframe_connectionId.clear(); + interframe_connectionStrength.clear(); + + printf("connectedComponent: %5.5fs\n",getTime()-start_inf); +/* std::vector staticdata; std::vector dynamicdata; std::vector movingdata; @@ -2758,9 +2878,46 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); viewer->spin(); - } + cloud_sample->points.clear(); + for(unsigned int i = 0; i < current_point; i++){ + if(rand() % 4 == 0 && labels[i] == 0){ + cloud_sample->points.push_back(cloud->points[i]); + cloud_sample->points.back().r = 0; + cloud_sample->points.back().g = 0; + cloud_sample->points.back().b = 255; + } + } + for(unsigned int c = 0; c < component_dynamic.size(); c++){ + int randr = rand()%256; + int randg = rand()%256; + int randb = rand()%256; + for(unsigned int i = 0; i < component_dynamic[c].size(); i++){ + cloud_sample->points.push_back(cloud->points[component_dynamic[c][i]]); + cloud_sample->points.back().r = randr; + cloud_sample->points.back().g = randg; + cloud_sample->points.back().b = randb; + } + } + + for(unsigned int c = 0; c < component_moving.size(); c++){ + int randr = rand()%256; + int randg = rand()%256; + int randb = rand()%256; + for(unsigned int i = 0; i < component_moving[c].size(); i++){ + cloud_sample->points.push_back(cloud->points[component_moving[c][i]]); + cloud_sample->points.back().r = randr; + cloud_sample->points.back().g = randg; + cloud_sample->points.back().b = randb; + } + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + } +*/ delete[] priors; + printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime); } From f45cc32f3eb2373f85a5217184af43f067e0d2eb Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 26 Sep 2016 10:21:28 +0200 Subject: [PATCH 110/255] changes for porting to robot --- .../scripts/view_points_service.py | 2 +- quasimodo/quasimodo_brain/src/Util/Util.cpp | 40 ++--- .../src/segmentationserver.cpp | 3 +- .../include/modelupdater/ModelUpdater.h | 4 +- .../src/modelupdater/ModelUpdater.cpp | 163 +++++++++--------- .../include/semantic_map/semantic_map_node.h | 51 +++--- 6 files changed, 133 insertions(+), 130 deletions(-) diff --git a/object_view_generator/scripts/view_points_service.py b/object_view_generator/scripts/view_points_service.py index 53078b5e..3531f6d0 100755 --- a/object_view_generator/scripts/view_points_service.py +++ b/object_view_generator/scripts/view_points_service.py @@ -105,7 +105,7 @@ def generate_trajectory_points(self,req): try: rospy.loginfo("Getting a map...") msg = rospy.wait_for_message(self.map_frame, OccupancyGrid , - timeout=10.0) + timeout=90.0) rospy.loginfo("got map.") self.process_map(msg) except rospy.ROSException, e: diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 879743b8..c7671895 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -272,31 +272,31 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec for(int j = 0; j < models.size(); j++){ reglib::Model * model = models[j]; -// std::vector masks; -// for(unsigned int i = 0; i < model->frames.size(); i++){ -// reglib::RGBDFrame * frame = model->frames[i]; -// reglib::Camera * cam = frame->camera; -// cv::Mat mask; -// mask.create(cam->height,cam->width,CV_8UC1); -// unsigned char * maskdata = (unsigned char *)(mask.data); -// for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} -// masks.push_back(mask); -// } -// std::vector movemask; -// std::vector dynmask; -// printf("computeMovingDynamicStatic\n"); -// mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions -// external.push_back(movemask); -// internal.push_back(masks); -// dynamic.push_back(dynmask); - - SegmentationResults sr = mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions + std::vector masks; + for(unsigned int i = 0; i < model->frames.size(); i++){ + reglib::RGBDFrame * frame = model->frames[i]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + masks.push_back(mask); + } + std::vector movemask; + std::vector dynmask; + printf("computeMovingDynamicStatic\n"); + mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions + external.push_back(movemask); + internal.push_back(masks); + dynamic.push_back(dynmask); + + //SegmentationResults sr = mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions } delete reg; delete mu; printf("total segment time: %5.5fs\n",getTime()-startTime); - exit(0); + //exit(0); } } diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index 5057cacf..6a39c1cf 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -132,7 +132,7 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs for(unsigned int j = 0; j < models[i]->frames.size(); j++){ cv_bridge::CvImage dynamicmaskBridgeImage; dynamicmaskBridgeImage.image = dynamic[i][j]; - dynamicmaskBridgeImage.encoding = "mono8"; + dynamicmaskBridgeImage.encoding = "mono8"; res.dynamicmasks[i].images.push_back( *(dynamicmaskBridgeImage.toImageMsg()) ); cv_bridge::CvImage internalmaskBridgeImage; @@ -140,7 +140,6 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs internalmaskBridgeImage.encoding = "mono8"; res.movingmasks[i].images.push_back( *(internalmaskBridgeImage.toImageMsg()) ); } - res.models.push_back(quasimodo_brain::getModelMSG(models[i])); } diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 2d405025..4ca9166c 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -112,8 +112,8 @@ class ModelUpdater{ virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks); virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg = false); - //virtual void computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); - virtual SegmentationResults computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); + virtual void computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); + //virtual SegmentationResults computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); virtual OcclusionScore computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1, bool debugg = false); virtual OcclusionScore computeOcclusionScore(Model * mod, vector cp, vector cf, vector cm, Matrix4d rp = Matrix4d::Identity(), int step = 1, bool debugg = false); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index ca9db762..efcddb89 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2029,8 +2029,8 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & } -SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ -//void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ +//SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ +void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ double computeMovingDynamicStatic_startTime = getTime(); SegmentationResults sr; @@ -2101,7 +2101,7 @@ SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bg dfuncTMP->refine_mean = true; dfuncTMP->zeromean = false; //dfuncTMP->startreg = 0.0005; - dfuncTMP->startreg = 0.0; + dfuncTMP->startreg = 0.0001; dfuncTMP->debugg_print = false; dfuncTMP->bidir = true; //dfuncTMP->bidir = false; @@ -2366,7 +2366,12 @@ SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bg delete g; double end_inf = getTime(); - printf("static inference time: %10.10fs\n",end_inf-start_inf); + printf("static inference time: %10.10fs\n",end_inf-start_inf); std::vector< std::vector< unsigned long > > component_dynamic; + std::vector< double > scores_dynamic; + std::vector< double > total_dynamic; + std::vector< std::vector< unsigned long > > component_moving; + std::vector< double > scores_moving; + std::vector< double > total_moving; long dynamic_frameConnections = 0; @@ -2490,16 +2495,13 @@ SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bg const unsigned int pixels_per_image = width*height; const unsigned int nr_pixels = nr_frames*pixels_per_image; - std::vector< std::vector< unsigned long > > component_dynamic; - std::vector< double > scores_dynamic; - std::vector< double > total_dynamic; - std::vector< std::vector< unsigned long > > component_moving; - std::vector< double > scores_moving; - std::vector< double > total_moving; - double probthresh = 0.5; unsigned int number_of_dynamics = 0; + unsigned int nr_obj_dyn = 0; + unsigned int nr_obj_mov = 0; std::vector objectlabel; + std::vector labelID; + labelID.push_back(0); objectlabel.resize(nr_pixels); for(unsigned long i = 0; i < nr_pixels; i++){objectlabel[i] = 0;} for(unsigned long ind = 0; ind < nr_pixels; ind++){ @@ -2525,65 +2527,107 @@ SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bg double p1 = priors[3*cind+1]; double p2 = priors[3*cind+2]; - double s = 0; - if(p0 > p2){s += p1 - p0;} - else{ s += p1 - p2;} - score += s; - totsum++; + if(valids[cind]){ + double s = 0; + if(p0 > p2){s += p1 - p0;} + else{ s += p1 - p2;} + score += s; + totsum++; + } float * dedata = (float*)(frames[frameind]->de.data); int dir; dir = -1; - if(valids[cind+dir] && w > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + if( w > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ objectlabel[cind+dir] = number_of_dynamics; todo.push_back(cind+dir); } dir = 1; - if(valids[cind+dir] && w < (width-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + if( w < (width-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ objectlabel[cind+dir] = number_of_dynamics; todo.push_back(cind+dir); } dir = -width; - if(valids[cind+dir] && h > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + if( h > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ objectlabel[cind+dir] = number_of_dynamics; todo.push_back(cind+dir); } dir = width; - if(valids[cind+dir] && h < (height-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + if( h < (height-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ objectlabel[cind+dir] = number_of_dynamics; todo.push_back(cind+dir); } for(unsigned long j = 0; j < interframe_connectionId[cind].size();j++){ unsigned long other = interframe_connectionId[cind][j]; - if(valids[other] && objectlabel[other] == 0 && labels[other] == current_label){ + if( objectlabel[other] == 0 && labels[other] == current_label){ objectlabel[other] = number_of_dynamics; todo.push_back(other); } } } + + + + labelID.push_back(0); + if(score < 0){continue;} if(current_label == 1){ + labelID.back() = ++nr_obj_dyn; printf("Dynamic: %f -> %f\n",score,totsum); - component_dynamic.push_back(todo); - scores_dynamic.push_back(score); - total_dynamic.push_back(totsum); + sr.component_dynamic.push_back(todo); + sr.scores_dynamic.push_back(score); + sr.total_dynamic.push_back(totsum); } if(current_label == 2){ + labelID.back() = --nr_obj_mov; printf("Moving: %f -> %f\n",score,totsum); - component_moving.push_back(todo); - scores_moving.push_back(score); - total_moving.push_back(totsum); + sr.component_moving.push_back(todo); + sr.scores_moving.push_back(score); + sr.total_moving.push_back(totsum); } + printf("back: %i\n",labelID.back()); } } interframe_connectionId.clear(); interframe_connectionStrength.clear(); printf("connectedComponent: %5.5fs\n",getTime()-start_inf); + + int current = 0; + for(unsigned long i = 0; i < frames.size(); i++){ + Camera * camera = frames[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + cv::Mat m; + m.create(height,width,CV_8UC1); + unsigned char * mdata = (unsigned char*)m.data; + + cv::Mat d; + d.create(height,width,CV_8UC1); + unsigned char * ddata = (unsigned char*)d.data; + for(int j = 0; j < width*height; j++){ + mdata[j] = 0; + ddata[j] = 0; + unsigned int label = labels[current]; + int lid = labelID[label]; + if(lid > 0){ + ddata[j] = lid; + }else if(lid < 0){ + mdata[j] = -lid; + } + current++; + } + movemask.push_back(m); + dynmask.push_back(d); + } + + + /* std::vector staticdata; std::vector dynamicdata; @@ -2603,7 +2647,7 @@ SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bg if(depthdata[j] != 0){ if(labels[current] == 0){ staticdata.push_back(current); - staticcloud->points.push_back(cloud->points[current]); + staticcloud->points.push_back(cloud->pointmovemask,dynmask,s[current]); } if(labels[current] == 1){ @@ -2694,7 +2738,7 @@ SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bg moving_ec.setClusterTolerance (0.05); // 5cm moving_ec.setMinClusterSize (1); moving_ec.setMaxClusterSize (250000000); - moving_ec.setSearchMethod (movingtree); + moving_ec.setSearchMethod (movingtree);movemask,dynmask, moving_ec.setInputCloud (movingcloud); moving_ec.extract (moving_indices); @@ -2812,7 +2856,7 @@ SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bg movemask.push_back(m); dynmask.push_back(d); } - +*/ if(debugg){ pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); @@ -2833,50 +2877,6 @@ SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bg viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); viewer->spin(); - printf("results-class\n"); - for(unsigned int i = 0; i < current_point; i++){ - if(labels[i] == 0){ - cloud->points[i].r = 0; - cloud->points[i].g = 0; - cloud->points[i].b = 255; - } - - if(labels[i] == 1){ - cloud->points[i].r = 0; - cloud->points[i].g = 0; - cloud->points[i].b = 0; - } - - if(labels[i] == 2){ - cloud->points[i].r = 0; - cloud->points[i].g = 0; - cloud->points[i].b = 0; - } - - if(labels[i] == 3){ - cloud->points[i].r = 0; - cloud->points[i].g = 255; - cloud->points[i].b = 0; - } - - if(labels[i] == 4){ - cloud->points[i].r = 255; - cloud->points[i].g = 0; - cloud->points[i].b = 0; - } - } - - - - cloud_sample->points.clear(); - for(unsigned int i = 0; i < current_point; i++){ - if(rand() % 4 == 0){ - cloud_sample->points.push_back(cloud->points[i]); - } - } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); - viewer->spin(); cloud_sample->points.clear(); for(unsigned int i = 0; i < current_point; i++){ @@ -2888,24 +2888,24 @@ SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bg } } - for(unsigned int c = 0; c < component_dynamic.size(); c++){ + for(unsigned int c = 0; c < sr.component_dynamic.size(); c++){ int randr = rand()%256; int randg = rand()%256; int randb = rand()%256; - for(unsigned int i = 0; i < component_dynamic[c].size(); i++){ - cloud_sample->points.push_back(cloud->points[component_dynamic[c][i]]); + for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){ + cloud_sample->points.push_back(cloud->points[sr.component_dynamic[c][i]]); cloud_sample->points.back().r = randr; cloud_sample->points.back().g = randg; cloud_sample->points.back().b = randb; } } - for(unsigned int c = 0; c < component_moving.size(); c++){ + for(unsigned int c = 0; c < sr.component_moving.size(); c++){ int randr = rand()%256; int randg = rand()%256; int randb = rand()%256; - for(unsigned int i = 0; i < component_moving[c].size(); i++){ - cloud_sample->points.push_back(cloud->points[component_moving[c][i]]); + for(unsigned int i = 0; i < sr.component_moving[c].size(); i++){ + cloud_sample->points.push_back(cloud->points[sr.component_moving[c][i]]); cloud_sample->points.back().r = randr; cloud_sample->points.back().g = randg; cloud_sample->points.back().b = randb; @@ -2915,9 +2915,10 @@ SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bg viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); viewer->spin(); } -*/ + delete[] priors; printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime); + //return sr; } diff --git a/semantic_map/include/semantic_map/semantic_map_node.h b/semantic_map/include/semantic_map/semantic_map_node.h index 07b8b6a5..719b1260 100644 --- a/semantic_map/include/semantic_map/semantic_map_node.h +++ b/semantic_map/include/semantic_map/semantic_map_node.h @@ -297,6 +297,7 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam meta_parser.saveMetaRoomAsXML(*metaroom); } + CloudPtr dynamicClusters(new Cloud()); CloudPtr difference(new Cloud()); if(segmentationtype == 1){ std::string previousObservationXml; @@ -366,7 +367,8 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam unsigned char * maskdata = mask.data; for(unsigned int j = 0; j < currentcloudTMP->points.size(); j++){ if(maskdata[j] > 0){ - difference->points.push_back(currentcloudTMP->points[j]); + //difference->points.push_back(currentcloudTMP->points[j]); + dynamicClusters->points.push_back(currentcloudTMP->points[j]); } } } @@ -530,7 +532,7 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam ROS_INFO_STREAM("Comparison cloud "<points.size()<<" room cloud "<points.size()); } -} +//} ROS_INFO_STREAM("Raw difference "<points.size()); /* @@ -553,35 +555,36 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam ROS_INFO_STREAM("Clustered differences. "< seg; - pcl::PointIndices::Ptr inliers (new pcl::PointIndices); - pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); - - seg.setOptimizeCoefficients (true); - seg.setModelType (pcl::SACMODEL_PLANE); - seg.setMethodType (pcl::SAC_RANSAC); - seg.setMaxIterations (100); - seg.setDistanceThreshold (0.02); - - seg.setInputCloud (vClusters[i]); - seg.segment (*inliers, *coefficients); - if (inliers->indices.size () > 0.9 * vClusters[i]->points.size()) - { - ROS_INFO_STREAM("Discarding planar dynamic cluster"); - } else { - *dynamicClusters += *vClusters[i]; - } - } + for (size_t i=0; i seg; + pcl::PointIndices::Ptr inliers (new pcl::PointIndices); + pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); + + seg.setOptimizeCoefficients (true); + seg.setModelType (pcl::SACMODEL_PLANE); + seg.setMethodType (pcl::SAC_RANSAC); + seg.setMaxIterations (100); + seg.setDistanceThreshold (0.02); + + seg.setInputCloud (vClusters[i]); + seg.segment (*inliers, *coefficients); + if (inliers->indices.size () > 0.9 * vClusters[i]->points.size()) + { + ROS_INFO_STREAM("Discarding planar dynamic cluster"); + } else { + *dynamicClusters += *vClusters[i]; + } + } */ for (size_t i=0; i Date: Mon, 26 Sep 2016 12:01:37 +0200 Subject: [PATCH 111/255] fixing semantic map integration with segmentationsever --- semantic_map/launch/semantic_map.launch | 6 ++++++ semantic_map/src/semantic_map_main.cpp | 7 +------ 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/semantic_map/launch/semantic_map.launch b/semantic_map/launch/semantic_map.launch index f420ae33..493a6d41 100644 --- a/semantic_map/launch/semantic_map.launch +++ b/semantic_map/launch/semantic_map.launch @@ -9,6 +9,12 @@ + + + + + + diff --git a/semantic_map/src/semantic_map_main.cpp b/semantic_map/src/semantic_map_main.cpp index 2014ff7c..2f3082db 100644 --- a/semantic_map/src/semantic_map_main.cpp +++ b/semantic_map/src/semantic_map_main.cpp @@ -2,18 +2,13 @@ int main(int argc, char **argv) { - printf("test\n"); - // Set up ROS. ros::init(argc, argv, "Semantic_map_node"); ros::NodeHandle n; ros::NodeHandle aRosNode("~"); - printf("test\n"); - //SemanticMapNode aSemanticMapNode(aRosNode); SemanticMapNode aSemanticMapNode(n); aSemanticMapNode.processRoomObservation(argv[1]); - exit(0); - printf("test\n"); + ros::Rate loop_rate(10); while (ros::ok()) { From c92bf6d4b3f9f6e3601847889332b24ba5c4bdf9 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 26 Sep 2016 16:42:04 +0200 Subject: [PATCH 112/255] added segserver launch --- quasimodo/quasimodo_brain/launch/segmentationserver.launch | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 quasimodo/quasimodo_brain/launch/segmentationserver.launch diff --git a/quasimodo/quasimodo_brain/launch/segmentationserver.launch b/quasimodo/quasimodo_brain/launch/segmentationserver.launch new file mode 100644 index 00000000..735fb51c --- /dev/null +++ b/quasimodo/quasimodo_brain/launch/segmentationserver.launch @@ -0,0 +1,7 @@ + + + + + + + From 48c848aba8b4ea4dcc308a78ce4254933264fd5a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 26 Sep 2016 16:44:49 +0200 Subject: [PATCH 113/255] removed segserver from semantic map launcher --- semantic_map/launch/semantic_map.launch | 6 ------ 1 file changed, 6 deletions(-) diff --git a/semantic_map/launch/semantic_map.launch b/semantic_map/launch/semantic_map.launch index 493a6d41..7b27d1bc 100644 --- a/semantic_map/launch/semantic_map.launch +++ b/semantic_map/launch/semantic_map.launch @@ -10,12 +10,6 @@ - - - - - - From 71651fd7da5f927bb812e2edcb65147890d80ca8 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 27 Sep 2016 14:24:57 +0200 Subject: [PATCH 114/255] ... --- .../scripts/view_points_service.py | 2 +- .../src/segment_room_pairs.cpp | 2 ++ .../src/segmentationserver.cpp | 35 ++++++++++++------- .../src/modelupdater/ModelUpdater.cpp | 20 ++++++++--- .../src/registration/MassRegistrationPPR2.cpp | 12 +++---- .../include/semantic_map/semantic_map_node.h | 2 ++ 6 files changed, 49 insertions(+), 24 deletions(-) diff --git a/object_view_generator/scripts/view_points_service.py b/object_view_generator/scripts/view_points_service.py index 3531f6d0..3eb286a0 100755 --- a/object_view_generator/scripts/view_points_service.py +++ b/object_view_generator/scripts/view_points_service.py @@ -105,7 +105,7 @@ def generate_trajectory_points(self,req): try: rospy.loginfo("Getting a map...") msg = rospy.wait_for_message(self.map_frame, OccupancyGrid , - timeout=90.0) + timeout=120.0) rospy.loginfo("got map.") self.process_map(msg) except rospy.ROSException, e: diff --git a/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp index 9595583f..6e4debf7 100644 --- a/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp +++ b/quasimodo/quasimodo_brain/src/segment_room_pairs.cpp @@ -100,11 +100,13 @@ bool segment_metaroom(quasimodo_msgs::metaroom_pair::Request & req, quasimodo_m } int main(int argc, char** argv){ + ROS_INFO("starting segment_room_pairs"); ros::init(argc, argv, "segmentationserver_metaroom"); ros::NodeHandle n; segmentation_client = n.serviceClient("segment_model"); ros::ServiceServer service = n.advertiseService("segment_metaroom", segment_metaroom); + ROS_INFO("running segment_room_pairs"); ros::spin(); /* reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index 6a39c1cf..dd5366b7 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -52,9 +52,10 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; - quasimodo_brain::segment(bg,models,internal,external,dynamic); + quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization); + printf("visualization: %i\n",visualization); - for(unsigned int i = 0; visualization && i < models.size(); i++){ + for(unsigned int i = 0; false && visualization && i < models.size(); i++){ std::vector internal_masks = internal[i]; std::vector external_masks = external[i]; std::vector dynamic_masks = dynamic[i]; @@ -85,7 +86,6 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs const unsigned int width = camera->width; const unsigned int height = camera->height; - for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; @@ -93,16 +93,13 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs if(z > 0){ float x = (float(w) - cx) * z * ifx; float y = (float(h) - cy) * z * ify; - pcl::PointXYZRGBNormal point; point.x = m00*x + m01*y + m02*z + m03; point.y = m10*x + m11*y + m12*z + m13; point.z = m20*x + m21*y + m22*z + m23; - point.b = rgbdata[3*ind+0]; point.g = rgbdata[3*ind+1]; point.r = rgbdata[3*ind+2]; - if(dynamicmaskdata[ind] != 0){ point.b = 0; point.g = 255; @@ -112,7 +109,6 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs point.g = 0; point.r = 255; } - cloud->points.push_back(point); } } @@ -121,9 +117,20 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs viewer->removeAllPointClouds(); viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); viewer->spin(); - //while(cv::waitKey(50)!='q'){viewer->spinOnce();} } +// for(unsigned int i = 0; visualization && i < models.size(); i++){ +// std::vector internal_masks = internal[i]; +// std::vector external_masks = external[i]; +// std::vector dynamic_masks = dynamic[i]; +// reglib::Model * model = models[i]; +// for(unsigned int j = 0; j < model->frames.size(); j++){ +// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", models[i]->frames[j]->rgb); +// cv::namedWindow( "moving", cv::WINDOW_AUTOSIZE ); cv::imshow( "moving", 255*(internal[i][j] > 0)); +// cv::namedWindow( "dynamic", cv::WINDOW_AUTOSIZE ); cv::imshow( "dynamic", 255*(dynamic[i][j] > 0)); +// cv::waitKey(0); +// } +// } res.backgroundmodel = req.backgroundmodel; res.dynamicmasks.resize(models.size()); @@ -139,6 +146,11 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs internalmaskBridgeImage.image = internal[i][j]; internalmaskBridgeImage.encoding = "mono8"; res.movingmasks[i].images.push_back( *(internalmaskBridgeImage.toImageMsg()) ); + +// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", models[i]->frames[j]->rgb); +// cv::namedWindow( "moving", cv::WINDOW_AUTOSIZE ); cv::imshow( "moving", 255*(internal[i][j] > 0)); +// cv::namedWindow( "dynamic", cv::WINDOW_AUTOSIZE ); cv::imshow( "dynamic", 255*(dynamic[i][j] > 0)); +// cv::waitKey(0); } res.models.push_back(quasimodo_brain::getModelMSG(models[i])); } @@ -157,11 +169,9 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs } int main(int argc, char** argv){ - +ROS_INFO("starting segmentserver."); ros::init(argc, argv, "segmentationserver"); ros::NodeHandle n; - - int inputstate = -1; for(int i = 1; i < argc;i++){ printf("input: %s\n",argv[i]); if(std::string(argv[i]).compare("-v") == 0){ printf("visualization turned on\n"); visualization = true;} @@ -173,9 +183,8 @@ int main(int argc, char** argv){ viewer->setBackgroundColor(0.0,0.0,0.0); } - ros::ServiceServer service = n.advertiseService("segment_model", segment_model); - ROS_INFO("Ready to add use segment_model."); +ROS_INFO("Ready to add use segment_model."); ros::spin(); } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index efcddb89..4cf2b06b 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2101,7 +2101,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dfuncTMP->refine_mean = true; dfuncTMP->zeromean = false; //dfuncTMP->startreg = 0.0005; - dfuncTMP->startreg = 0.0001; + dfuncTMP->startreg = 0.00001; dfuncTMP->debugg_print = false; dfuncTMP->bidir = true; //dfuncTMP->bidir = false; @@ -2574,7 +2574,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s labelID.push_back(0); - if(score < 0){continue;} + if(score < 200){continue;} if(current_label == 1){ labelID.back() = ++nr_obj_dyn; printf("Dynamic: %f -> %f\n",score,totsum); @@ -2589,7 +2589,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s sr.scores_moving.push_back(score); sr.total_moving.push_back(totsum); } - printf("back: %i\n",labelID.back()); + //printf("back: %i\n",labelID.back()); } } interframe_connectionId.clear(); @@ -2610,10 +2610,16 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s cv::Mat d; d.create(height,width,CV_8UC1); unsigned char * ddata = (unsigned char*)d.data; + + cv::Mat d2; + d2.create(height,width,CV_8UC1); + unsigned char * ddata2 = (unsigned char*)d2.data; + for(int j = 0; j < width*height; j++){ mdata[j] = 0; ddata[j] = 0; - unsigned int label = labels[current]; + ddata2[j] = labels[current]; + unsigned int label = objectlabel[current]; int lid = labelID[label]; if(lid > 0){ ddata[j] = lid; @@ -2624,6 +2630,12 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } movemask.push_back(m); dynmask.push_back(d); + +// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frames[i]->rgb); +// cv::namedWindow( "moving", cv::WINDOW_AUTOSIZE ); cv::imshow( "moving", 255*(m > 0)); +// cv::namedWindow( "dynamic", cv::WINDOW_AUTOSIZE ); cv::imshow( "dynamic", 255*(d > 0)); +// cv::namedWindow( "dynamic2", cv::WINDOW_AUTOSIZE ); cv::imshow( "dynamic2", 255*(d2 > 0)); +// cv::waitKey(0); } diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 59b7e2fb..a8f76168 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -293,7 +293,7 @@ void MassRegistrationPPR2::addModel(Model * model){ depthedge_trees3d[i]->buildIndex(); } */ - printf("addModel total load time: %5.5f points: %6.6i\n",getTime()-total_load_time_start,count); + //printf("addModel total load time: %5.5f points: %6.6i\n",getTime()-total_load_time_start,count); } void MassRegistrationPPR2::addModelData(Model * model_, bool submodels){ @@ -3310,11 +3310,11 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectornoiseval > 10.0*func->regularization && ratio > 0.75){break;} } - printf("total_time: %5.5f\n",getTime()-total_time_start); - printf("rematch_time: %5.5f\n",rematch_time); - printf("compM residuals_time:%5.5f\n",residuals_time); - printf("computeModel: %5.5f\n",computeModel_time); - printf("opt_time: %5.5f\n",opt_time); +// printf("total_time: %5.5f\n",getTime()-total_time_start); +// printf("rematch_time: %5.5f\n",rematch_time); +// printf("compM residuals_time:%5.5f\n",residuals_time); +// printf("computeModel: %5.5f\n",computeModel_time); +// printf("opt_time: %5.5f\n",opt_time); // printf("setup_matches_time: %5.5f\n",setup_matches_time); // printf("setup_equation_time: %5.5f\n",setup_equation_time); diff --git a/semantic_map/include/semantic_map/semantic_map_node.h b/semantic_map/include/semantic_map/semantic_map_node.h index 719b1260..36a001fb 100644 --- a/semantic_map/include/semantic_map/semantic_map_node.h +++ b/semantic_map/include/semantic_map/semantic_map_node.h @@ -373,6 +373,7 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam } } + // std::vector vClusters = MetaRoom::clusterPointCloud(dynamiccloud,0.03,100,1000000); // ROS_INFO_STREAM("Clustered differences. "<::processRoomObservation(std::string xml_file_nam } } + printf("dynamicClusters->points.size() = %i\n",dynamicClusters->points.size()); // publish dynamic clusters // transform back into the sweep frame of ref (before metaroom registration, to align with the previously published sweep point cloud) Eigen::Matrix4f inverseRoomTransform = aRoom.getRoomTransform().inverse(); From 28978a0655056c0edbf4a6504404c5fe4b74bc3c Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 27 Sep 2016 16:07:08 +0200 Subject: [PATCH 115/255] ... --- .../include/semantic_map/semantic_map_node.h | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/semantic_map/include/semantic_map/semantic_map_node.h b/semantic_map/include/semantic_map/semantic_map_node.h index 36a001fb..efb20318 100644 --- a/semantic_map/include/semantic_map/semantic_map_node.h +++ b/semantic_map/include/semantic_map/semantic_map_node.h @@ -297,6 +297,15 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam meta_parser.saveMetaRoomAsXML(*metaroom); } + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + printf("%i\n",__LINE__); + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + usleep(10*1000*1000); + CloudPtr dynamicClusters(new Cloud()); CloudPtr difference(new Cloud()); if(segmentationtype == 1){ @@ -348,6 +357,15 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam sm.request.background = previousObservationXml; sm.request.foreground = xml_file_name; + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + printf("%i\n",__LINE__); + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + + usleep(10*1000*1000); if (m_segmentation_client.call(sm)){ CloudPtr dynamiccloud(new Cloud()); std::vector dynamicmasks; @@ -384,6 +402,15 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam return; } + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + printf("%i\n",__LINE__); + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + printf("//////////////////////////////////////////////////////////\n"); + usleep(10*1000*1000); + }else{ // this stores the difference between a) current obs and previous obs or b) current obs and metaroom (depending on the node parameters) From fe1556dee6681ef7105be4dbb2d17d77e73eea00 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 27 Sep 2016 16:20:47 +0200 Subject: [PATCH 116/255] ... --- semantic_map/include/semantic_map/semantic_map_node.h | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/semantic_map/include/semantic_map/semantic_map_node.h b/semantic_map/include/semantic_map/semantic_map_node.h index efb20318..6f332917 100644 --- a/semantic_map/include/semantic_map/semantic_map_node.h +++ b/semantic_map/include/semantic_map/semantic_map_node.h @@ -186,6 +186,16 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam { std::cout<<"File name "< Date: Thu, 29 Sep 2016 11:41:17 +0200 Subject: [PATCH 117/255] pulled out msgs and services from object managre to allow for multiple implementations of services --- object_manager/CMakeLists.txt | 29 +-- object_manager/package.xml | 2 + .../src/dynamic_object_utilities.cpp | 1 + object_manager_msgs/CMakeLists.txt | 183 ++++++++++++++++++ .../msg/DynamicObjectTrackingData.msg | 3 + .../msg/DynamicObjectTracks.msg | 2 + object_manager_msgs/package.xml | 58 ++++++ .../srv/DynamicObjectComputeMaskService.srv | 4 + .../srv/DynamicObjectsService.srv | 5 + .../srv/GetDynamicObjectService.srv | 8 + .../srv/ProcessDynamicObjectService.srv | 3 + 11 files changed, 273 insertions(+), 25 deletions(-) create mode 100644 object_manager_msgs/CMakeLists.txt create mode 100644 object_manager_msgs/msg/DynamicObjectTrackingData.msg create mode 100644 object_manager_msgs/msg/DynamicObjectTracks.msg create mode 100644 object_manager_msgs/package.xml create mode 100644 object_manager_msgs/srv/DynamicObjectComputeMaskService.srv create mode 100644 object_manager_msgs/srv/DynamicObjectsService.srv create mode 100644 object_manager_msgs/srv/GetDynamicObjectService.srv create mode 100644 object_manager_msgs/srv/ProcessDynamicObjectService.srv diff --git a/object_manager/CMakeLists.txt b/object_manager/CMakeLists.txt index 21e492a5..aebe50b2 100644 --- a/object_manager/CMakeLists.txt +++ b/object_manager/CMakeLists.txt @@ -4,7 +4,7 @@ project(object_manager) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs geometry_msgs pcl_ros message_generation qt_build cv_bridge semantic_map metaroom_xml_parser convex_segmentation k_means_tree observation_registration_services) +find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs geometry_msgs object_manager_msgs pcl_ros message_generation qt_build cv_bridge semantic_map metaroom_xml_parser convex_segmentation k_means_tree observation_registration_services) set(CMAKE_CXX_FLAGS "-O4 -fPIC -std=c++0x -fpermissive ${CMAKE_CXX_FLAGS}") @@ -25,27 +25,6 @@ endif() rosbuild_prepare_qt4(QtCore QtXml) -add_service_files( - FILES - DynamicObjectsService.srv - GetDynamicObjectService.srv - DynamicObjectComputeMaskService.srv - ProcessDynamicObjectService.srv -) - -add_message_files( - FILES - DynamicObjectTracks.msg - DynamicObjectTrackingData.msg -) - -generate_messages( - DEPENDENCIES - std_msgs - sensor_msgs - geometry_msgs -) - catkin_package( INCLUDE_DIRS include LIBRARIES dynamic_object @@ -76,13 +55,13 @@ include_directories(include add_library(dynamic_object ${HDRS} ${SRCS}) add_executable(object_manager_node ${HDRS} ${SRCS} include/object_manager/object_manager.h src/object_manager.cpp src/object_manager_main.cpp) -add_dependencies(object_manager_node object_manager_node_generate_messages_cpp) +add_dependencies(object_manager_node quasimodo_msgs_generate_messages_cpp object_manager_node_generate_messages_cpp) add_executable(load_objects_from_mongo ${HDRS} ${SRCS} src/load_objects_from_mongo.cpp) -add_dependencies(load_objects_from_mongo load_objects_from_mongo_generate_messages_cpp) +add_dependencies(load_objects_from_mongo quasimodo_msgs_generate_messages_cpp load_objects_from_mongo_generate_messages_cpp) add_executable(dynamic_object_compute_mask_server ${HDRS} ${SRCS} src/dynamic_object_compute_mask_server.cpp) -add_dependencies(dynamic_object_compute_mask_server object_manager_generate_messages_cpp observation_registration_services_generate_messages_cpp) +add_dependencies(dynamic_object_compute_mask_server quasimodo_msgs_generate_messages_cpp object_manager_generate_messages_cpp observation_registration_services_generate_messages_cpp) target_link_libraries(dynamic_object ${catkin_LIBRARIES} diff --git a/object_manager/package.xml b/object_manager/package.xml index bebc1ab2..e349122b 100644 --- a/object_manager/package.xml +++ b/object_manager/package.xml @@ -28,8 +28,10 @@ convex_segmentation k_means_tree observation_registration_services + object_manager_msgs + object_manager_msgs roscpp std_msgs sensor_msgs diff --git a/object_manager/src/dynamic_object_utilities.cpp b/object_manager/src/dynamic_object_utilities.cpp index 97d308f7..b7e59132 100644 --- a/object_manager/src/dynamic_object_utilities.cpp +++ b/object_manager/src/dynamic_object_utilities.cpp @@ -5,6 +5,7 @@ using namespace std; std::vector dynamic_object_utilities::loadDynamicObjects(std::string folder, bool verbose) { + printf("loadDynamicObjects\n"); std::vector objects; folder+=std::string("/"); diff --git a/object_manager_msgs/CMakeLists.txt b/object_manager_msgs/CMakeLists.txt new file mode 100644 index 00000000..f2cec13c --- /dev/null +++ b/object_manager_msgs/CMakeLists.txt @@ -0,0 +1,183 @@ +cmake_minimum_required(VERSION 2.8.3) +project(object_manager_msgs) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS std_msgs sensor_msgs geometry_msgs message_generation) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +add_service_files( + FILES + DynamicObjectsService.srv + GetDynamicObjectService.srv + DynamicObjectComputeMaskService.srv + ProcessDynamicObjectService.srv +) + +add_message_files( + FILES + DynamicObjectTracks.msg + DynamicObjectTrackingData.msg +) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs # Or other packages containing msgs + sensor_msgs + geometry_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES object_manager_msgs + CATKIN_DEPENDS message_runtime +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) + +## Declare a C++ library +# add_library(quasimodo_msgs +# src/${PROJECT_NAME}/quasimodo_msgs.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(quasimodo_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(quasimodo_msgs_node src/quasimodo_msgs_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(quasimodo_msgs_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(quasimodo_msgs_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS quasimodo_msgs quasimodo_msgs_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_quasimodo_msgs.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/object_manager_msgs/msg/DynamicObjectTrackingData.msg b/object_manager_msgs/msg/DynamicObjectTrackingData.msg new file mode 100644 index 00000000..66a46dc3 --- /dev/null +++ b/object_manager_msgs/msg/DynamicObjectTrackingData.msg @@ -0,0 +1,3 @@ +geometry_msgs/Transform[] additional_view_transforms +sensor_msgs/PointCloud2[] additional_views +int32[] object_mask # in first view diff --git a/object_manager_msgs/msg/DynamicObjectTracks.msg b/object_manager_msgs/msg/DynamicObjectTracks.msg new file mode 100644 index 00000000..ec358671 --- /dev/null +++ b/object_manager_msgs/msg/DynamicObjectTracks.msg @@ -0,0 +1,2 @@ +geometry_msgs/Transform[] poses +sensor_msgs/PointCloud2[] clouds diff --git a/object_manager_msgs/package.xml b/object_manager_msgs/package.xml new file mode 100644 index 00000000..f8b13cb6 --- /dev/null +++ b/object_manager_msgs/package.xml @@ -0,0 +1,58 @@ + + + object_manager_msgs + 0.0.0 + The quasimodo_msgs package + + + + + nbore + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + std_msgs + sensor_msgs + geometry_msgs + message_generation + + std_msgs + sensor_msgs + geometry_msgs + message_runtime + + + + + + + diff --git a/object_manager_msgs/srv/DynamicObjectComputeMaskService.srv b/object_manager_msgs/srv/DynamicObjectComputeMaskService.srv new file mode 100644 index 00000000..c0e4a8ab --- /dev/null +++ b/object_manager_msgs/srv/DynamicObjectComputeMaskService.srv @@ -0,0 +1,4 @@ +string object_xml +string observation_xml +--- +sensor_msgs/PointCloud2 segmented_object diff --git a/object_manager_msgs/srv/DynamicObjectsService.srv b/object_manager_msgs/srv/DynamicObjectsService.srv new file mode 100644 index 00000000..155f839c --- /dev/null +++ b/object_manager_msgs/srv/DynamicObjectsService.srv @@ -0,0 +1,5 @@ +string waypoint_id +--- +string[] object_id +sensor_msgs/PointCloud2[] objects +geometry_msgs/Point[] centroids diff --git a/object_manager_msgs/srv/GetDynamicObjectService.srv b/object_manager_msgs/srv/GetDynamicObjectService.srv new file mode 100644 index 00000000..169f2d50 --- /dev/null +++ b/object_manager_msgs/srv/GetDynamicObjectService.srv @@ -0,0 +1,8 @@ +string waypoint_id +string object_id +--- +sensor_msgs/PointCloud2 object_cloud +int32[] object_mask +geometry_msgs/Transform transform_to_map +int32 pan_angle +int32 tilt_angle diff --git a/object_manager_msgs/srv/ProcessDynamicObjectService.srv b/object_manager_msgs/srv/ProcessDynamicObjectService.srv new file mode 100644 index 00000000..51027c52 --- /dev/null +++ b/object_manager_msgs/srv/ProcessDynamicObjectService.srv @@ -0,0 +1,3 @@ +string observation_xml +string object_xml +--- From 6bd3710a7170c53dfd17991ecabb0441d125dd5c Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 29 Sep 2016 14:01:23 +0200 Subject: [PATCH 118/255] removed old msgs --- .../include/object_manager/object_manager.h | 42 +++++++++---------- .../msg/DynamicObjectTrackingData.msg | 3 -- object_manager/msg/DynamicObjectTracks.msg | 2 - .../srv/DynamicObjectComputeMaskService.srv | 4 -- object_manager/srv/DynamicObjectsService.srv | 5 --- .../srv/GetDynamicObjectService.srv | 8 ---- .../srv/ProcessDynamicObjectService.srv | 3 -- 7 files changed, 21 insertions(+), 46 deletions(-) delete mode 100644 object_manager/msg/DynamicObjectTrackingData.msg delete mode 100644 object_manager/msg/DynamicObjectTracks.msg delete mode 100644 object_manager/srv/DynamicObjectComputeMaskService.srv delete mode 100644 object_manager/srv/DynamicObjectsService.srv delete mode 100644 object_manager/srv/GetDynamicObjectService.srv delete mode 100644 object_manager/srv/ProcessDynamicObjectService.srv diff --git a/object_manager/include/object_manager/object_manager.h b/object_manager/include/object_manager/object_manager.h index cd96d074..5642c6ec 100644 --- a/object_manager/include/object_manager/object_manager.h +++ b/object_manager/include/object_manager/object_manager.h @@ -20,19 +20,19 @@ #include // Services -#include -#include -#include +#include +#include +#include // Registration service #include // Additional view mask service -#include +#include // Custom messages -#include -#include +#include +#include // PCL includes #include @@ -69,14 +69,14 @@ class ObjectManager { typedef typename Cloud::Ptr CloudPtr; typedef semantic_map_load_utilties::DynamicObjectData ObjectData; - typedef typename object_manager::DynamicObjectsService::Request DynamicObjectsServiceRequest; - typedef typename object_manager::DynamicObjectsService::Response DynamicObjectsServiceResponse; + typedef typename object_manager_msgs::DynamicObjectsService::Request DynamicObjectsServiceRequest; + typedef typename object_manager_msgs::DynamicObjectsService::Response DynamicObjectsServiceResponse; - typedef typename object_manager::GetDynamicObjectService::Request GetDynamicObjectServiceRequest; - typedef typename object_manager::GetDynamicObjectService::Response GetDynamicObjectServiceResponse; + typedef typename object_manager_msgs::GetDynamicObjectService::Request GetDynamicObjectServiceRequest; + typedef typename object_manager_msgs::GetDynamicObjectService::Response GetDynamicObjectServiceResponse; - typedef typename object_manager::ProcessDynamicObjectService::Request ProcessDynamicObjectServiceRequest; - typedef typename object_manager::ProcessDynamicObjectService::Response ProcessDynamicObjectServiceResponse; + typedef typename object_manager_msgs::ProcessDynamicObjectService::Request ProcessDynamicObjectServiceRequest; + typedef typename object_manager_msgs::ProcessDynamicObjectService::Response ProcessDynamicObjectServiceResponse; struct GetObjStruct { @@ -98,7 +98,7 @@ class ObjectManager { bool returnObjectMask(std::string waypoint, std::string object_id, std::string observation_xml, GetObjStruct& returned_object); void additionalViewsCallback(const sensor_msgs::PointCloud2::ConstPtr& msg); void additionalViewsStatusCallback(const std_msgs::String& msg); - void dynamicObjectTracksCallback(const object_manager::DynamicObjectTracksConstPtr& msg); + void dynamicObjectTracksCallback(const object_manager_msgs::DynamicObjectTracksConstPtr& msg); static CloudPtr filterGroundClusters(CloudPtr dynamic, double min_height) { @@ -167,7 +167,7 @@ ObjectManager::ObjectManager(ros::NodeHandle nh) : m_TransformListene m_PublisherRequestedObjectCloud = m_NodeHandle.advertise("/object_manager/requested_object", 1, true); m_PublisherRequestedObjectImage = m_NodeHandle.advertise("/object_manager/requested_object_mask", 1, true); m_PublisherLearnedObjectXml = m_NodeHandle.advertise("/object_learning/learned_object_xml", 1, false); - m_PublisherLearnedObjectTrackingData = m_NodeHandle.advertise("/object_learning/learned_object_tracking_data", 1, false); + m_PublisherLearnedObjectTrackingData = m_NodeHandle.advertise("/object_learning/learned_object_tracking_data", 1, false); m_PublisherLearnedObjectModel = m_NodeHandle.advertise("/object_learning/learned_object_model", 1, false); m_DynamicObjectsServiceServer = m_NodeHandle.advertiseService("ObjectManager/DynamicObjectsService", &ObjectManager::dynamicObjectsServiceCallback, this); @@ -297,8 +297,8 @@ void ObjectManager::additionalViewsStatusCallback(const std_msgs::Str } // create additional view masks - ros::ServiceClient client_masks = m_NodeHandle.serviceClient("/dynamic_object_compute_mask_server"); - object_manager::DynamicObjectComputeMaskService srv_masks; + ros::ServiceClient client_masks = m_NodeHandle.serviceClient("/dynamic_object_compute_mask_server"); + object_manager_msgs::DynamicObjectComputeMaskService srv_masks; srv_masks.request.observation_xml = m_objectTrackedObservation; srv_masks.request.object_xml = xml_file; @@ -320,7 +320,7 @@ void ObjectManager::additionalViewsStatusCallback(const std_msgs::Str // re-load object and publish tracks data ObjectData object = semantic_map_load_utilties::loadDynamicObjectFromSingleSweep(xml_file); - object_manager::DynamicObjectTrackingData tracking_data_msg; + object_manager_msgs::DynamicObjectTrackingData tracking_data_msg; // views for (auto obj_view : object.vAdditionalViews){ @@ -407,7 +407,7 @@ void ObjectManager::additionalViewsCallback(const sensor_msgs::PointC } template -void ObjectManager::dynamicObjectTracksCallback(const object_manager::DynamicObjectTracksConstPtr& msg) +void ObjectManager::dynamicObjectTracksCallback(const object_manager_msgs::DynamicObjectTracksConstPtr& msg) { if (m_objectTracked == NULL) { @@ -624,8 +624,8 @@ bool ObjectManager::processDynamicObjectServiceCallback(ProcessDynami // } // create additional view masks - ros::ServiceClient client_masks = m_NodeHandle.serviceClient("/dynamic_object_compute_mask_server"); - object_manager::DynamicObjectComputeMaskService srv_masks; + ros::ServiceClient client_masks = m_NodeHandle.serviceClient("/dynamic_object_compute_mask_server"); + object_manager_msgs::DynamicObjectComputeMaskService srv_masks; srv_masks.request.observation_xml = req.observation_xml; srv_masks.request.object_xml = req.object_xml; @@ -639,7 +639,7 @@ bool ObjectManager::processDynamicObjectServiceCallback(ProcessDynami // re-load object and publish tracks data ObjectData processed_object = semantic_map_load_utilties::loadDynamicObjectFromSingleSweep(req.object_xml); - object_manager::DynamicObjectTrackingData tracking_data_msg; + object_manager_msgs::DynamicObjectTrackingData tracking_data_msg; // views for (auto obj_view : processed_object.vAdditionalViews){ diff --git a/object_manager/msg/DynamicObjectTrackingData.msg b/object_manager/msg/DynamicObjectTrackingData.msg deleted file mode 100644 index 66a46dc3..00000000 --- a/object_manager/msg/DynamicObjectTrackingData.msg +++ /dev/null @@ -1,3 +0,0 @@ -geometry_msgs/Transform[] additional_view_transforms -sensor_msgs/PointCloud2[] additional_views -int32[] object_mask # in first view diff --git a/object_manager/msg/DynamicObjectTracks.msg b/object_manager/msg/DynamicObjectTracks.msg deleted file mode 100644 index ec358671..00000000 --- a/object_manager/msg/DynamicObjectTracks.msg +++ /dev/null @@ -1,2 +0,0 @@ -geometry_msgs/Transform[] poses -sensor_msgs/PointCloud2[] clouds diff --git a/object_manager/srv/DynamicObjectComputeMaskService.srv b/object_manager/srv/DynamicObjectComputeMaskService.srv deleted file mode 100644 index c0e4a8ab..00000000 --- a/object_manager/srv/DynamicObjectComputeMaskService.srv +++ /dev/null @@ -1,4 +0,0 @@ -string object_xml -string observation_xml ---- -sensor_msgs/PointCloud2 segmented_object diff --git a/object_manager/srv/DynamicObjectsService.srv b/object_manager/srv/DynamicObjectsService.srv deleted file mode 100644 index 155f839c..00000000 --- a/object_manager/srv/DynamicObjectsService.srv +++ /dev/null @@ -1,5 +0,0 @@ -string waypoint_id ---- -string[] object_id -sensor_msgs/PointCloud2[] objects -geometry_msgs/Point[] centroids diff --git a/object_manager/srv/GetDynamicObjectService.srv b/object_manager/srv/GetDynamicObjectService.srv deleted file mode 100644 index 169f2d50..00000000 --- a/object_manager/srv/GetDynamicObjectService.srv +++ /dev/null @@ -1,8 +0,0 @@ -string waypoint_id -string object_id ---- -sensor_msgs/PointCloud2 object_cloud -int32[] object_mask -geometry_msgs/Transform transform_to_map -int32 pan_angle -int32 tilt_angle diff --git a/object_manager/srv/ProcessDynamicObjectService.srv b/object_manager/srv/ProcessDynamicObjectService.srv deleted file mode 100644 index 51027c52..00000000 --- a/object_manager/srv/ProcessDynamicObjectService.srv +++ /dev/null @@ -1,3 +0,0 @@ -string observation_xml -string object_xml ---- From ba632e8228bf8ffbb7a919f551fba685503b8471 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 29 Sep 2016 14:03:46 +0200 Subject: [PATCH 119/255] broke out msgs and srv from semantic_map --- semantic_map/CMakeLists.txt | 36 +++++++-------- .../include/semantic_map/semantic_map_node.h | 17 ++++--- semantic_map/package.xml | 2 + semantic_map_msgs/CMakeLists.txt | 27 +++++++++++ semantic_map_msgs/README.md | 0 .../msg/RoomObservation.msg | 0 semantic_map_msgs/package.xml | 45 +++++++++++++++++++ .../srv/ClearMetaroomService.srv | 0 .../srv/DynamicClusterService.srv | 0 .../srv/MetaroomService.srv | 0 .../srv/ObservationService.srv | 0 11 files changed, 102 insertions(+), 25 deletions(-) create mode 100644 semantic_map_msgs/CMakeLists.txt create mode 100644 semantic_map_msgs/README.md rename {semantic_map => semantic_map_msgs}/msg/RoomObservation.msg (100%) create mode 100644 semantic_map_msgs/package.xml rename {semantic_map => semantic_map_msgs}/srv/ClearMetaroomService.srv (100%) rename {semantic_map => semantic_map_msgs}/srv/DynamicClusterService.srv (100%) rename {semantic_map => semantic_map_msgs}/srv/MetaroomService.srv (100%) rename {semantic_map => semantic_map_msgs}/srv/ObservationService.srv (100%) diff --git a/semantic_map/CMakeLists.txt b/semantic_map/CMakeLists.txt index 3f489311..4f102928 100644 --- a/semantic_map/CMakeLists.txt +++ b/semantic_map/CMakeLists.txt @@ -4,7 +4,7 @@ project(semantic_map) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS quasimodo_msgs mongodb_store roscpp rospy std_msgs sensor_msgs pcl_ros message_generation qt_build image_geometry cv_bridge tf_conversions strands_sweep_registration metaroom_xml_parser observation_registration_services) +find_package(catkin REQUIRED COMPONENTS quasimodo_msgs semantic_map_msgs mongodb_store roscpp rospy std_msgs sensor_msgs pcl_ros message_generation qt_build image_geometry cv_bridge tf_conversions strands_sweep_registration metaroom_xml_parser observation_registration_services) find_package(Boost REQUIRED COMPONENTS system thread program_options filesystem) set(CMAKE_CXX_FLAGS "-O4 -fPIC -std=c++0x -fpermissive ${CMAKE_CXX_FLAGS}") @@ -18,21 +18,21 @@ add_definitions(${PCL_DEFINITIONS}) rosbuild_prepare_qt4(QtCore QtXml) -add_message_files( - FILES - RoomObservation.msg -) - -add_service_files( - FILES - ClearMetaroomService.srv -) - -generate_messages( - DEPENDENCIES - std_msgs - sensor_msgs -) +#add_message_files( +# FILES +# RoomObservation.msg +#) + +#add_service_files( +# FILES +# ClearMetaroomService.srv +#) + +#generate_messages( +# DEPENDENCIES +# std_msgs +# sensor_msgs +#) catkin_package( INCLUDE_DIRS include @@ -98,8 +98,8 @@ add_executable(semantic_map_node include/semantic_map/semantic_map_node.h src/se add_executable(load_from_mongo src/load_from_mongo.cpp) add_executable(add_to_mongo src/add_to_mongo.cpp) -add_dependencies(semantic_map quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) -add_dependencies(semantic_map_node quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) +add_dependencies(semantic_map semantic_map_msgs_generate_messages_cpp quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) +add_dependencies(semantic_map_node semantic_map_msgs_generate_messages_cpp quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) #add_dependencies(semantic_map_recompute quasimodo_msgs_generate_messages_cpp semantic_map_generate_messages_cpp primitive_extraction_generate_messages_cpp strands_perception_msgs_generate_messages_cpp observation_registration_services_generate_messages_cpp) target_link_libraries(semantic_map diff --git a/semantic_map/include/semantic_map/semantic_map_node.h b/semantic_map/include/semantic_map/semantic_map_node.h index 6f332917..0601282f 100644 --- a/semantic_map/include/semantic_map/semantic_map_node.h +++ b/semantic_map/include/semantic_map/semantic_map_node.h @@ -14,16 +14,19 @@ #include -#include +#include #include // Services -#include +#include #include //#include //#include -#include + +//#include "object_manager/dynamic_object.h" + +#include // PCL includes #include @@ -69,15 +72,15 @@ class SemanticMapNode { typedef typename std::map >::iterator WaypointRoomMapIterator; typedef typename std::map::iterator WaypointPointCloudMapIterator; - typedef typename semantic_map::ClearMetaroomService::Request ClearMetaroomServiceRequest; - typedef typename semantic_map::ClearMetaroomService::Response ClearMetaroomServiceResponse; + typedef typename semantic_map_msgs::ClearMetaroomService::Request ClearMetaroomServiceRequest; + typedef typename semantic_map_msgs::ClearMetaroomService::Response ClearMetaroomServiceResponse; SemanticMapNode(ros::NodeHandle nh); ~SemanticMapNode(); void processRoomObservation(std::string xml_file_name); - void roomObservationCallback(const semantic_map::RoomObservationConstPtr& obs_msg); + void roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& obs_msg); bool clearMetaroomServiceCallback(ClearMetaroomServiceRequest &req, ClearMetaroomServiceResponse &res); ros::Subscriber m_SubscriberRoomObservation; @@ -673,7 +676,7 @@ void SemanticMapNode::processRoomObservation(std::string xml_file_nam } template -void SemanticMapNode::roomObservationCallback(const semantic_map::RoomObservationConstPtr& obs_msg) +void SemanticMapNode::roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& obs_msg) { std::cout<<"Room obs message received"<processRoomObservation(obs_msg->xml_file_name); diff --git a/semantic_map/package.xml b/semantic_map/package.xml index 080dcfe0..df218cbd 100644 --- a/semantic_map/package.xml +++ b/semantic_map/package.xml @@ -28,8 +28,10 @@ strands_sweep_registration metaroom_xml_parser observation_registration_services + semantic_map_msgs + semantic_map_msgs rospy roscpp std_msgs diff --git a/semantic_map_msgs/CMakeLists.txt b/semantic_map_msgs/CMakeLists.txt new file mode 100644 index 00000000..ab3a39fd --- /dev/null +++ b/semantic_map_msgs/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 2.8.3) +project(semantic_map_msgs) + +find_package(catkin REQUIRED COMPONENTS std_msgs sensor_msgs geometry_msgs message_generation) + +add_message_files( + FILES + RoomObservation.msg +) + +add_service_files( + FILES + ClearMetaroomService.srv +) + +generate_messages( + DEPENDENCIES + std_msgs + sensor_msgs +) + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES semantic_map_msgs + CATKIN_DEPENDS message_runtime +# DEPENDS system_lib +) diff --git a/semantic_map_msgs/README.md b/semantic_map_msgs/README.md new file mode 100644 index 00000000..e69de29b diff --git a/semantic_map/msg/RoomObservation.msg b/semantic_map_msgs/msg/RoomObservation.msg similarity index 100% rename from semantic_map/msg/RoomObservation.msg rename to semantic_map_msgs/msg/RoomObservation.msg diff --git a/semantic_map_msgs/package.xml b/semantic_map_msgs/package.xml new file mode 100644 index 00000000..b1f38924 --- /dev/null +++ b/semantic_map_msgs/package.xml @@ -0,0 +1,45 @@ + + + semantic_map_msgs + 0.0.0 + The semantic map msgs package + + Johan Ekekrantz + + + MIT + + + catkin + rospy + roscpp + std_msgs + sensor_msgs + message_generation + libqt4-dev + qt_build + qt_ros + mongodb_store + libpcl-all-dev + image_geometry + pcl_ros + cv_bridge + + rospy + roscpp + std_msgs + sensor_msgs + message_runtime + libqt4-dev + qt_build + qt_ros + mongodb_store + libpcl-all + pcl_ros + cv_bridge + tf_conversions + image_geometry + + + + diff --git a/semantic_map/srv/ClearMetaroomService.srv b/semantic_map_msgs/srv/ClearMetaroomService.srv similarity index 100% rename from semantic_map/srv/ClearMetaroomService.srv rename to semantic_map_msgs/srv/ClearMetaroomService.srv diff --git a/semantic_map/srv/DynamicClusterService.srv b/semantic_map_msgs/srv/DynamicClusterService.srv similarity index 100% rename from semantic_map/srv/DynamicClusterService.srv rename to semantic_map_msgs/srv/DynamicClusterService.srv diff --git a/semantic_map/srv/MetaroomService.srv b/semantic_map_msgs/srv/MetaroomService.srv similarity index 100% rename from semantic_map/srv/MetaroomService.srv rename to semantic_map_msgs/srv/MetaroomService.srv diff --git a/semantic_map/srv/ObservationService.srv b/semantic_map_msgs/srv/ObservationService.srv similarity index 100% rename from semantic_map/srv/ObservationService.srv rename to semantic_map_msgs/srv/ObservationService.srv From 5db8499bc032316e6538540927c8410b7700ab1a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 29 Sep 2016 15:29:58 +0200 Subject: [PATCH 120/255] ... --- quasimodo/quasimodo_brain/CMakeLists.txt | 26 ++------------ quasimodo/quasimodo_brain/package.xml | 6 ++-- .../metaroom_additional_view_processing.cpp | 34 ++++++++++++++++++- .../quasimodo_models/src/model/Model.cpp | 3 +- 4 files changed, 40 insertions(+), 29 deletions(-) diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 53ca5d7b..a31771bf 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(quasimodo_brain) -find_package(catkin REQUIRED COMPONENTS roscpp soma_msgs quasimodo_msgs quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge) +find_package(catkin REQUIRED COMPONENTS roscpp soma_msgs object_manager_msgs semantic_map_msgs quasimodo_msgs quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge) set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fPIC -std=c++0x -o popcnt -mssse3") @@ -17,30 +17,8 @@ include_directories(${catkin_INCLUDE_DIRS}) # This is the root dir of https://github.com/nilsbore/dynamic_object_retrieval set(retrieval_project_dir ${CMAKE_CURRENT_SOURCE_DIR}/../../dynamic_object_retrieval/) - -# This is the base vocabulary tree representation -#set(k_means_tree_dir ${retrieval_project_dir}/k_means_tree) -#set(k_means_tree_library_dir ${k_means_tree_dir}/build) -#include_directories(${k_means_tree_dir}/include ${k_means_tree_dir}/impl) -#link_directories(${k_means_tree_library_dir}) - -# This is the stopwatch by Whelan, also included in the base dir -#set(stopwatch_dir ${retrieval_dir}/stopwatch) -#set(stopwatch_include_dir ${stopwatch_dir}/src) -#include_directories(${stopwatch_include_dir}) - find_package(OpenGL) -# The benchmark project is also in the base dir -#set(benchmark_dir ${retrieval_project_dir}/benchmark) -#set(benchmark_libraries benchmark_result benchmark_retrieval benchmark_visualization benchmark_overlap surfel_renderer ${OPENGL_gl_LIBRARY} ${QT_QTOPENGL_LIBRARY}) -#include_directories(${benchmark_dir}/include) -#link_directories(${benchmark_dir}/build) - -# Define the locations of cereal serialization header files -#set(cereal_include_dir ${k_means_tree_dir}/cereal/include) -#include_directories(${cereal_include_dir}) - ############################################## ##################### INCLUDES ############### ############################################## @@ -158,7 +136,7 @@ target_link_libraries( segmentationserver quasimodo_brain_util quasimodo_ModelDa add_executable( metaroom_additional_view_processing src/metaroom_additional_view_processing.cpp) -add_dependencies( metaroom_additional_view_processing roscpp quasimodo_msgs_generate_messages_cpp) +add_dependencies( metaroom_additional_view_processing roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp) target_link_libraries( metaroom_additional_view_processing quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) diff --git a/quasimodo/quasimodo_brain/package.xml b/quasimodo/quasimodo_brain/package.xml index 2b255500..15c938bd 100644 --- a/quasimodo/quasimodo_brain/package.xml +++ b/quasimodo/quasimodo_brain/package.xml @@ -56,8 +56,10 @@ message_runtime eigen_conversions cv_bridge - semantic_map - semantic_map + semantic_map_msgs + object_manager_msgs + object_manager_msgs + semantic_map_msgs quasimodo_models quasimodo_msgs metaroom_xml_parser diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 1bb85d53..9d4d3647 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -62,6 +62,32 @@ #include "core/RGBDFrame.h" #include "Util/Util.h" +// Services +#include +#include +#include + +// Registration service +#include + +// Additional view mask service +#include + +// Custom messages +#include +#include + +ros::ServiceServer m_DynamicObjectsServiceServer; + +typedef typename object_manager_msgs::DynamicObjectsService::Request DynamicObjectsServiceRequest; +typedef typename object_manager_msgs::DynamicObjectsService::Response DynamicObjectsServiceResponse; + +typedef typename object_manager_msgs::GetDynamicObjectService::Request GetDynamicObjectServiceRequest; +typedef typename object_manager_msgs::GetDynamicObjectService::Response GetDynamicObjectServiceResponse; + +typedef typename object_manager_msgs::ProcessDynamicObjectService::Request ProcessDynamicObjectServiceRequest; +typedef typename object_manager_msgs::ProcessDynamicObjectService::Response ProcessDynamicObjectServiceResponse; + ros::ServiceClient segmentation_client; using namespace std; using namespace semantic_map_load_utilties; @@ -610,7 +636,6 @@ std::vector readPoseXML(std::string xmlFile){ } delete xmlReader; printf("done readPoseXML\n"); - //exit(0); return poses; } @@ -1050,6 +1075,11 @@ void sendCallback(const std_msgs::String::ConstPtr& msg){ sendMetaroomToServer(msg->data); } +bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res){ + printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n"); + return false; +} + int main(int argc, char** argv){ @@ -1139,6 +1169,8 @@ int main(int argc, char** argv){ } } + m_DynamicObjectsServiceServer = m_NodeHandle.advertiseService("ObjectManager/DynamicObjectsService", &ObjectManager::dynamicObjectsServiceCallback, this); + if(!once){ros::spin();} printf("done...\n"); return 0; diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 16188c03..70ecfd6c 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -517,8 +517,7 @@ void Model::fullDelete(){ all_keypoints.clear(); all_descriptors.clear(); relativeposes.clear(); - for(size_t i = 0; i < frames.size(); i++){ - printf("delete camera: %ld\n",frames[i]->camera); + for(size_t i = 0; i < frames.size(); i++){ delete frames[i]->camera; delete frames[i]; } From 627cbf9150ab489cc3536a25e577f6b8575a4f5e Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 29 Sep 2016 17:27:16 +0200 Subject: [PATCH 121/255] fixing issues --- .../quasimodo_brain/src/metaroom_additional_view_processing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 9d4d3647..d24195fb 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1169,7 +1169,7 @@ int main(int argc, char** argv){ } } - m_DynamicObjectsServiceServer = m_NodeHandle.advertiseService("ObjectManager/DynamicObjectsService", &ObjectManager::dynamicObjectsServiceCallback, this); + m_DynamicObjectsServiceServer = n.advertiseService("ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback); if(!once){ros::spin();} printf("done...\n"); From 46f158e40ee6f3f473cb8eacacbf9bb8fb265744 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 29 Sep 2016 17:35:21 +0200 Subject: [PATCH 122/255] removing object manager starting... --- semantic_map_launcher/launch/semantic_map.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/semantic_map_launcher/launch/semantic_map.launch b/semantic_map_launcher/launch/semantic_map.launch index f2e26a42..8ff05e42 100644 --- a/semantic_map_launcher/launch/semantic_map.launch +++ b/semantic_map_launcher/launch/semantic_map.launch @@ -51,13 +51,13 @@ - + From bbd80cda15d51e621fd0df2349a9440b6345ada4 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 29 Sep 2016 18:18:40 +0200 Subject: [PATCH 123/255] ... --- .../quasimodo_brain/src/metaroom_additional_view_processing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index d24195fb..743562f8 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1169,7 +1169,7 @@ int main(int argc, char** argv){ } } - m_DynamicObjectsServiceServer = n.advertiseService("ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback); + m_DynamicObjectsServiceServer = n.advertiseService("/object_manager_node/ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback); if(!once){ros::spin();} printf("done...\n"); From 5c578eb58c27156b71f1abd91604e0824acb2c9b Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 29 Sep 2016 19:13:30 +0200 Subject: [PATCH 124/255] getDynamic... --- .../src/metaroom_additional_view_processing.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 743562f8..21cfb2ed 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -78,6 +78,7 @@ #include ros::ServiceServer m_DynamicObjectsServiceServer; +ros::ServiceServer m_GetDynamicObjectServiceServer; typedef typename object_manager_msgs::DynamicObjectsService::Request DynamicObjectsServiceRequest; typedef typename object_manager_msgs::DynamicObjectsService::Response DynamicObjectsServiceResponse; @@ -1080,6 +1081,11 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj return false; } +bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res){ + printf("bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res)\n"); + return false; +} + int main(int argc, char** argv){ @@ -1170,7 +1176,7 @@ int main(int argc, char** argv){ } m_DynamicObjectsServiceServer = n.advertiseService("/object_manager_node/ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback); - + m_GetDynamicObjectServiceServer = n.advertiseService("/object_manager_node/ObjectManager/GetDynamicObjectService", getDynamicObjectServiceCallback); if(!once){ros::spin();} printf("done...\n"); return 0; From 5392cc6355081e8abc900cf4da6cf5c331b99b99 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 29 Sep 2016 23:09:39 +0200 Subject: [PATCH 125/255] ... --- .../metaroom_additional_view_processing.cpp | 104 +++++++++++++++++- .../quasimodo_models/src/core/RGBDFrame.cpp | 28 ++--- 2 files changed, 115 insertions(+), 17 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 21cfb2ed..3240e6d3 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -697,8 +697,7 @@ void processMetaroom(std::string path){ std::vector< std::vector< cv::Mat > > dynamic; quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization_lvl > 0); - bg->fullDelete(); - delete bg; + remove_old_seg(sweep_folder); @@ -725,7 +724,8 @@ void processMetaroom(std::string path){ for(unsigned int j = 0; j < mod_fr.size(); j++){ reglib::RGBDFrame * frame = mod_fr[j]; - Eigen::Matrix4d p = mod_po[j]; + std::cout << mod_po[j] << std::endl; + Eigen::Matrix4d p = mod_po[j]*bg.front()->pose; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); unsigned short * depthdata = (unsigned short *)(frame->depth.data); float * normalsdata = (float *)(frame->normals.data); @@ -813,12 +813,23 @@ void processMetaroom(std::string path){ std::vector< std::vector > inds; inds.resize(mod_fr.size()); + double sumx = 0; + double sumy = 0; + double sumz = 0; pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ int pid = dynamic_indices[d].indices[ind]; inds[dynamic_frameid[pid]].push_back(dynamic_pixelid[pid]); cloud_cluster->points.push_back(dynamiccloud->points[pid]); + sumx += cloud_cluster->points.back().x; + sumy += cloud_cluster->points.back().y; + sumz += cloud_cluster->points.back().z; } + + sumx /= double(cloud_cluster->points.size()); + sumy /= double(cloud_cluster->points.size()); + sumz /= double(cloud_cluster->points.size()); + char buf [1024]; sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),d); QFile file(buf); @@ -830,6 +841,22 @@ void processMetaroom(std::string path){ xmlWriter->writeStartDocument(); xmlWriter->writeStartElement("Object"); xmlWriter->writeAttribute("object_number", QString::number(d)); + xmlWriter->writeStartElement("Mean"); + xmlWriter->writeAttribute("x", QString::number(sumx)); + xmlWriter->writeAttribute("y", QString::number(sumy)); + xmlWriter->writeAttribute("z", QString::number(sumz)); + xmlWriter->writeEndElement(); + +// token = xmlReader->readNext(); +// double x = atof(xmlReader->readElementText().toStdString().c_str()); + +// token = xmlReader->readNext(); +// double y = atof(xmlReader->readElementText().toStdString().c_str()); + +// token = xmlReader->readNext(); +// double z = atof(xmlReader->readElementText().toStdString().c_str()); + +// printf("mean: %f %f %f\n",x,y,z); for(unsigned int j = 0; j < mod_fr.size(); j++){ if(inds[j].size() == 0){continue;} @@ -925,6 +952,8 @@ void processMetaroom(std::string path){ } } + bg->fullDelete(); + delete bg; fullmodel->fullDelete(); delete fullmodel; // delete bgmassreg; @@ -1076,8 +1105,77 @@ void sendCallback(const std_msgs::String::ConstPtr& msg){ sendMetaroomToServer(msg->data); } +//string waypoint_id +//--- +//string[] object_id +//sensor_msgs/PointCloud2[] objects +//geometry_msgs/Point[] centroids + + bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res){ printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n"); + std::string path = req.waypoint_id; + processMetaroom(path); + + printf("path: %s\n",path.c_str()); + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + printf("sweep_folder: %s\n",sweep_folder.c_str()); + + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_object*.xml")); + for (auto objectFile : objectFiles){ + std::string object = sweep_folder+objectFile.toStdString(); + printf("object: %s\n",object.c_str()); + + QFile file(object.c_str()); + + if (!file.exists()){ + ROS_ERROR("Could not open file %s to masks.",object.c_str()); + continue; + } + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()) + { + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()) + { + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + break; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement) + { + + if (xmlReader->name() == "Mean") + { + token = xmlReader->readNext(); + double x = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext(); + double y = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext(); + double z = atof(xmlReader->readElementText().toStdString().c_str()); + + printf("mean: %f %f %f\n",x,y,z); + } + } + } + delete xmlReader; + } + return false; } diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index bf24c520..628a9069 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -828,8 +828,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt for(unsigned int w = 1; w < width-1;w++){ for(unsigned int h = 1; h < height-1;h++){ int ind = h*width+w; - maxima_dxdata[ind] = (src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] > src_dxdata[ind+1]); - maxima_dydata[ind] = (src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] > src_dydata[ind+width]); + maxima_dxdata[ind] = (src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] >= src_dxdata[ind+1]); + maxima_dydata[ind] = (src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] >= src_dydata[ind+width]); } } @@ -928,18 +928,18 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt detdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5)); } - // cv::namedWindow( "de", cv::WINDOW_AUTOSIZE ); cv::imshow( "de", de); - // cv::namedWindow( "re", cv::WINDOW_AUTOSIZE ); cv::imshow( "re", re); - // cv::namedWindow( "ge", cv::WINDOW_AUTOSIZE ); cv::imshow( "ge", ge); - // cv::namedWindow( "be", cv::WINDOW_AUTOSIZE ); cv::imshow( "be", be); - // cv::namedWindow( "ce", cv::WINDOW_AUTOSIZE ); cv::imshow( "ce", ce); - // cv::namedWindow( "cenms", cv::WINDOW_AUTOSIZE ); cv::imshow( "cenms", cenms); - // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); - // cv::namedWindow( "blur", cv::WINDOW_AUTOSIZE ); cv::imshow( "blur", blur_rgb); - // //cv::namedWindow( "maximas", cv::WINDOW_AUTOSIZE ); cv::imshow( "blur", blur_rgb); - // cv::waitKey(0); - // cv::namedWindow( "det", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det); - // cv::waitKey(0); +// cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", de); +// cv::namedWindow( "re", cv::WINDOW_AUTOSIZE ); cv::imshow( "re", re); +// cv::namedWindow( "ge", cv::WINDOW_AUTOSIZE ); cv::imshow( "ge", ge); +// cv::namedWindow( "be", cv::WINDOW_AUTOSIZE ); cv::imshow( "be", be); +// cv::namedWindow( "colour", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour", ce); +// cv::namedWindow( "colour+nms", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour+nms", cenms); +// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); +// cv::namedWindow( "blur", cv::WINDOW_AUTOSIZE ); cv::imshow( "blur", blur_rgb); +// //cv::namedWindow( "maximas", cv::WINDOW_AUTOSIZE ); cv::imshow( "blur", blur_rgb); +// cv::waitKey(0); +// // cv::namedWindow( "det", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det); +// // cv::waitKey(0); int dilation_size = 4; From 9fef87562c3373bbab0521865056fc381bd37cac Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 3 Oct 2016 15:52:19 +0200 Subject: [PATCH 126/255] ... --- .../metaroom_additional_view_processing.cpp | 318 +++++++++++++++--- 1 file changed, 273 insertions(+), 45 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 3240e6d3..4ae2f1d0 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -116,6 +116,9 @@ std::vector sweepPoses; #include #include +bool testDynamicObjectServiceCallback(std::string path); +bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res); + void remove_old_seg(std::string sweep_folder){ DIR *dir; struct dirent *ent; @@ -125,23 +128,23 @@ void remove_old_seg(std::string sweep_folder){ std::string file = std::string(ent->d_name); if (file.find("dynamic_object") !=std::string::npos && file.find(".xml") !=std::string::npos){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); } if (file.find("dynamicmask") !=std::string::npos && file.find(".png") !=std::string::npos){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); } if (file.find("moving_object") !=std::string::npos && file.find(".xml") !=std::string::npos){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); } if (file.find("movingmask") !=std::string::npos && file.find(".png") !=std::string::npos){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); } } closedir (dir); @@ -642,6 +645,8 @@ std::vector readPoseXML(std::string xmlFile){ void processMetaroom(std::string path){ printf("processing: %s\n",path.c_str()); + testDynamicObjectServiceCallback(path); + exit(0); int slash_pos = path.find_last_of("/"); std::string sweep_folder = path.substr(0, slash_pos) + "/"; @@ -681,6 +686,7 @@ void processMetaroom(std::string path){ if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} } + if(prevind != -1){ std::string prev = sweep_xmls[prevind]; printf("prev: %s\n",prev.c_str()); @@ -712,6 +718,223 @@ void processMetaroom(std::string path){ std::vector mod_mm; model->getData(mod_po, mod_fr, mod_mm); + int dynamicCounter = 1; + while(true){ + double sum = 0; + double sumx = 0; + double sumy = 0; + double sumz = 0; + std::vector imgnr; + std::vector masks; + + for(unsigned int j = 0; j < mod_fr.size(); j++){ + reglib::RGBDFrame * frame = mod_fr[j]; + //std::cout << mod_po[j] << std::endl; + Eigen::Matrix4d p = mod_po[j]*bg->frames.front()->pose; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + reglib::Camera * camera = frame->camera; + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); + unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + cv::Mat mask; + mask.create(height,width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + + + bool containsData = false; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + + if(dynamicmaskdata[ind] == dynamicCounter){ + maskdata[ind] = 255; + containsData = true; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + sumx += m00*x + m01*y + m02*z + m03; + sumy += m10*x + m11*y + m12*z + m13; + sumz += m20*x + m21*y + m22*z + m23; + sum ++; + } + }else{ + maskdata[ind] = 0; + } + } + } + + if(containsData){ + masks.push_back(mask); + imgnr.push_back(j); + } + } + if(masks.size() > 0){ + char buf [1024]; + sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),dynamicCounter-1); + QFile file(buf); + if (file.exists()){file.remove();} + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Object"); + xmlWriter->writeAttribute("object_number", QString::number(dynamicCounter-1)); + xmlWriter->writeStartElement("Mean"); + xmlWriter->writeAttribute("x", QString::number(sumx/sum)); + xmlWriter->writeAttribute("y", QString::number(sumy/sum)); + xmlWriter->writeAttribute("z", QString::number(sumz/sum)); + xmlWriter->writeEndElement(); + + + for(unsigned int j = 0; j < masks.size(); j++){ + char buf [1024]; + sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),dynamicCounter-1,imgnr[j]); + cv::imwrite(buf, masks[j] ); + + sprintf(buf,"dynamicmask_%i_%i.png",dynamicCounter-1,imgnr[j]); + xmlWriter->writeStartElement("Mask"); + xmlWriter->writeAttribute("filename", QString(buf)); + xmlWriter->writeAttribute("image_number", QString::number(imgnr[j])); + xmlWriter->writeEndElement(); + } + + xmlWriter->writeEndElement(); + xmlWriter->writeEndDocument(); + delete xmlWriter; + dynamicCounter++; + }else{break;} + } + + int movingCounter = 1; + while(true){ + double sum = 0; + double sumx = 0; + double sumy = 0; + double sumz = 0; + std::vector imgnr; + std::vector masks; + + for(unsigned int j = 0; j < mod_fr.size(); j++){ + reglib::RGBDFrame * frame = mod_fr[j]; + //std::cout << mod_po[j] << std::endl; + Eigen::Matrix4d p = mod_po[j]*bg->frames.front()->pose; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + reglib::Camera * camera = frame->camera; + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); + unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + cv::Mat mask; + mask.create(height,width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + + + bool containsData = false; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + + if(internalmaskdata[ind] == movingCounter){ + maskdata[ind] = 255; + containsData = true; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + sumx += m00*x + m01*y + m02*z + m03; + sumy += m10*x + m11*y + m12*z + m13; + sumz += m20*x + m21*y + m22*z + m23; + sum ++; + } + }else{ + maskdata[ind] = 0; + } + } + } + + if(containsData){ + masks.push_back(mask); + imgnr.push_back(j); + } + } + if(masks.size() > 0){ + char buf [1024]; + sprintf(buf,"%s/moving_object%10.10i.xml",sweep_folder.c_str(),movingCounter-1); + QFile file(buf); + if (file.exists()){file.remove();} + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Object"); + xmlWriter->writeAttribute("object_number", QString::number(movingCounter-1)); + xmlWriter->writeStartElement("Mean"); + xmlWriter->writeAttribute("x", QString::number(sumx/sum)); + xmlWriter->writeAttribute("y", QString::number(sumy/sum)); + xmlWriter->writeAttribute("z", QString::number(sumz/sum)); + xmlWriter->writeEndElement(); + + + for(unsigned int j = 0; j < masks.size(); j++){ + char buf [1024]; + sprintf(buf,"%s/movingmask_%i_%i.png",sweep_folder.c_str(),movingCounter-1,imgnr[j]); + cv::imwrite(buf, masks[j] ); + + sprintf(buf,"movingmask_%i_%i.png",movingCounter-1,imgnr[j]); + xmlWriter->writeStartElement("Mask"); + xmlWriter->writeAttribute("filename", QString(buf)); + xmlWriter->writeAttribute("image_number", QString::number(imgnr[j])); + xmlWriter->writeEndElement(); + } + + xmlWriter->writeEndElement(); + xmlWriter->writeEndDocument(); + delete xmlWriter; + movingCounter++; + }else{break;} + } + /* + return; + exit(0); + std::vector dynamic_frameid; std::vector dynamic_pixelid; @@ -725,7 +948,7 @@ void processMetaroom(std::string path){ for(unsigned int j = 0; j < mod_fr.size(); j++){ reglib::RGBDFrame * frame = mod_fr[j]; std::cout << mod_po[j] << std::endl; - Eigen::Matrix4d p = mod_po[j]*bg.front()->pose; + Eigen::Matrix4d p = mod_po[j]*bg->frames.front()->pose; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); unsigned short * depthdata = (unsigned short *)(frame->depth.data); float * normalsdata = (float *)(frame->normals.data); @@ -788,11 +1011,12 @@ void processMetaroom(std::string path){ } } -// if(visualization_lvl > 0 && cloud->points.size() > 0){ -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); -// viewer->spin(); -// } + + // if(visualization_lvl > 0 && cloud->points.size() > 0){ + // viewer->removeAllPointClouds(); + // viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); + // viewer->spin(); + // } printf("dynamiccloud: %i\n",int(dynamiccloud->points.size())); if(dynamiccloud->points.size() > 0){ @@ -847,16 +1071,16 @@ void processMetaroom(std::string path){ xmlWriter->writeAttribute("z", QString::number(sumz)); xmlWriter->writeEndElement(); -// token = xmlReader->readNext(); -// double x = atof(xmlReader->readElementText().toStdString().c_str()); + // token = xmlReader->readNext(); + // double x = atof(xmlReader->readElementText().toStdString().c_str()); -// token = xmlReader->readNext(); -// double y = atof(xmlReader->readElementText().toStdString().c_str()); + // token = xmlReader->readNext(); + // double y = atof(xmlReader->readElementText().toStdString().c_str()); -// token = xmlReader->readNext(); -// double z = atof(xmlReader->readElementText().toStdString().c_str()); + // token = xmlReader->readNext(); + // double z = atof(xmlReader->readElementText().toStdString().c_str()); -// printf("mean: %f %f %f\n",x,y,z); + // printf("mean: %f %f %f\n",x,y,z); for(unsigned int j = 0; j < mod_fr.size(); j++){ if(inds[j].size() == 0){continue;} @@ -948,12 +1172,15 @@ void processMetaroom(std::string path){ xmlWriter->writeEndDocument(); delete xmlWriter; } + } + */ } + bg->fullDelete(); + delete bg; } - bg->fullDelete(); - delete bg; + fullmodel->fullDelete(); delete fullmodel; // delete bgmassreg; @@ -1111,11 +1338,10 @@ void sendCallback(const std_msgs::String::ConstPtr& msg){ //sensor_msgs/PointCloud2[] objects //geometry_msgs/Point[] centroids - bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res){ printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n"); std::string path = req.waypoint_id; - processMetaroom(path); + //processMetaroom(path); printf("path: %s\n",path.c_str()); int slash_pos = path.find_last_of("/"); @@ -1141,34 +1367,23 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj QXmlStreamReader* xmlReader = new QXmlStreamReader(&file); - while (!xmlReader->atEnd() && !xmlReader->hasError()) - { + while (!xmlReader->atEnd() && !xmlReader->hasError()){ QXmlStreamReader::TokenType token = xmlReader->readNext(); if (token == QXmlStreamReader::StartDocument) continue; - if (xmlReader->hasError()) - { + if (xmlReader->hasError()){ ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); break; } QString elementName = xmlReader->name().toString(); - - if (token == QXmlStreamReader::StartElement) - { - - if (xmlReader->name() == "Mean") - { - token = xmlReader->readNext(); - double x = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext(); - double y = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext(); - double z = atof(xmlReader->readElementText().toStdString().c_str()); - + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "Mean"){ + QXmlStreamAttributes attributes = xmlReader->attributes(); + double x = atof(attributes.value("x").toString().toStdString().c_str()); + double y = atof(attributes.value("y").toString().toStdString().c_str()); + double z = atof(attributes.value("z").toString().toStdString().c_str()); printf("mean: %f %f %f\n",x,y,z); } } @@ -1180,8 +1395,21 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj } bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res){ + // DynamicObjectsServiceRequest req; + // req.waypoint_id = path; + // DynamicObjectsServiceResponse res; + // return dynamicObjectsServiceCallback(req,res); + return true; +} + + + +bool testDynamicObjectServiceCallback(std::string path){ printf("bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res)\n"); - return false; + DynamicObjectsServiceRequest req; + req.waypoint_id = path; + DynamicObjectsServiceResponse res; + return dynamicObjectsServiceCallback(req,res); } From d7ec1b7fdf67b348e964f97b451bbccab07c2b2d Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 4 Oct 2016 12:11:11 +0200 Subject: [PATCH 127/255] Added ability to have several indices --- quasimodo/quasimodo_msgs/msg/model.msg | 2 +- .../quasimodo_msgs/msg/retrieval_result.msg | 2 +- .../src/quasimodo_retrieval_node.cpp | 41 +++++++++++++++---- 3 files changed, 35 insertions(+), 10 deletions(-) diff --git a/quasimodo/quasimodo_msgs/msg/model.msg b/quasimodo/quasimodo_msgs/msg/model.msg index a6de3242..28f30d60 100644 --- a/quasimodo/quasimodo_msgs/msg/model.msg +++ b/quasimodo/quasimodo_msgs/msg/model.msg @@ -3,4 +3,4 @@ geometry_msgs/Pose[] local_poses rgbd_frame[] frames sensor_msgs/Image[] masks sensor_msgs/PointCloud2[] clouds -geometry_msgs/Pose[] global_pose +geometry_msgs/Pose global_pose diff --git a/quasimodo/quasimodo_msgs/msg/retrieval_result.msg b/quasimodo/quasimodo_msgs/msg/retrieval_result.msg index a013a2f6..e8a41456 100644 --- a/quasimodo/quasimodo_msgs/msg/retrieval_result.msg +++ b/quasimodo/quasimodo_msgs/msg/retrieval_result.msg @@ -7,4 +7,4 @@ string_array[] retrieved_image_paths float64[] retrieved_distance_scores int_array[] segment_indices geometry_msgs/Pose[] global_poses -uint64[] vocabulary_ids +string[] vocabulary_ids diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index f3414055..ba3f215b 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -521,6 +522,16 @@ class retrieval_node { return path[0]; } + vector vocabulary_set(const grouped_vocabulary_tree::result_type& vec) + { + return vec.subgroup_global_indices; + } + + vector vocabulary_set(const vocabulary_tree::result_type& ind) + { + return vector {ind.index}; + } + bool retrieval_implementation(const quasimodo_msgs::retrieval_query& query, quasimodo_msgs::retrieval_result& result, geometry_msgs::Transform& query_room_transform) { cout << "Received query msg of kind " << query.query_kind << "..." << endl; @@ -617,7 +628,7 @@ class retrieval_node { vector scores; vector indices; - vector vocabulary_ids; + vector > vocabulary_ids; for (auto s : results.first) { boost::filesystem::path segment_path = base_path(s.first); string name = segment_path.stem().string(); @@ -631,10 +642,12 @@ class retrieval_node { indices.push_back(index); } scores.push_back(s.second.score); - vocabulary_ids.push_back(s.second.index); + //vocabulary_ids.push_back(s.second.index); + vocabulary_ids.push_back(vocabulary_set(s.second)); } vector, Eigen::aligned_allocator > initial_poses; + vector > global_transforms; vector > masks(retrieved_clouds.size()); vector > images(retrieved_clouds.size()); vector > depths(retrieved_clouds.size()); @@ -643,15 +656,20 @@ class retrieval_node { vector inds; vector > sweep_transforms; if (indices[i] == -1) { // special case for mongodb result + global_transforms.push_back(Eigen::Matrix4f()); + global_transforms.back().setIdentity(); tie(masks[i], images[i], depths[i], inds) = generate_images_for_mongodb_object(retrieved_clouds[i], K, base_path(results.first[i].first), sweep_transforms); } else { - auto sweep_data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); + auto sweep_data = SimpleXMLParser::loadRoomFromXML(sweep_paths[i].string(), std::vector{"RoomIntermediateCloud"}, false, false); for (tf::StampedTransform& t : sweep_data.vIntermediateRoomCloudTransformsRegistered) { Eigen::Affine3d e; tf::transformTFToEigen(t, e); sweep_transforms.push_back(e.inverse().matrix().cast()); } + Eigen::Affine3d e; + tf::transformTFToEigen(sweep_data.vIntermediateRoomCloudTransforms[0], e); + global_transforms.push_back(e.matrix().cast()); tie(masks[i], images[i], depths[i], inds) = generate_images_for_object(retrieved_clouds[i], K, sweep_paths[i], sweep_transforms); } @@ -660,10 +678,8 @@ class retrieval_node { } } - - tf::transformTFToMsg(room_transform, query_room_transform); - result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices, vocabulary_ids); + result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices, vocabulary_ids, global_transforms); for (int i = 0; i < retrieved_clouds.size(); ++i) { sensor_msgs::PointCloud2 cloud_msg = result.retrieved_clouds[i]; cloud_msg.header.stamp = ros::Time::now(); @@ -752,7 +768,8 @@ class retrieval_node { const vector >& paths, const vector& scores, const vector& indices, - const vector& vocabulary_ids) + const vector >& vocabulary_ids, + const vector >& global_poses) { quasimodo_msgs::retrieval_result res; @@ -779,6 +796,7 @@ class retrieval_node { res.retrieved_distance_scores.resize(number_retrieved); res.segment_indices.resize(number_retrieved); res.vocabulary_ids.resize(number_retrieved); + res.global_poses.resize(number_retrieved); for (int i = 0; i < number_retrieved; ++i) { pcl::toROSMsg(*clouds[i], res.retrieved_clouds[i]); @@ -799,7 +817,14 @@ class retrieval_node { } res.retrieved_distance_scores[i] = scores[i]; res.segment_indices[i].ints.push_back(indices[i]); - res.vocabulary_ids[i] = vocabulary_ids[i]; + + stringstream ss; + for (int ind : vocabulary_ids[i]) { + cout << "Writing retrieval ind: " << ind << endl; + ss << ind << " "; + } + res.vocabulary_ids[i] = ss.str(); + tf::poseEigenToMsg(Eigen::Affine3d(global_poses[i].cast()), res.global_poses[i]); } return res; From 3b94d4e36d7ba7796eeaf6a695ab59ecdf1e2a71 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 16:56:13 +0200 Subject: [PATCH 128/255] quasimodo now replaces semantic map segmentation fully --- .../launch/learn_objects_dependencies.launch | 6 +- .../metaroom_additional_view_processing.cpp | 464 ++++++------------ 2 files changed, 146 insertions(+), 324 deletions(-) diff --git a/learn_objects_action/launch/learn_objects_dependencies.launch b/learn_objects_action/launch/learn_objects_dependencies.launch index 72d4dade..fee956c6 100644 --- a/learn_objects_action/launch/learn_objects_dependencies.launch +++ b/learn_objects_action/launch/learn_objects_dependencies.launch @@ -16,9 +16,9 @@ - - - + diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 4ae2f1d0..a44f4500 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -73,12 +73,19 @@ // Additional view mask service #include + +#include +#include + // Custom messages #include #include -ros::ServiceServer m_DynamicObjectsServiceServer; -ros::ServiceServer m_GetDynamicObjectServiceServer; + +#include + +std::vector< ros::ServiceServer > m_DynamicObjectsServiceServers; +std::vector< ros::ServiceServer > m_GetDynamicObjectServiceServers; typedef typename object_manager_msgs::DynamicObjectsService::Request DynamicObjectsServiceRequest; typedef typename object_manager_msgs::DynamicObjectsService::Response DynamicObjectsServiceResponse; @@ -102,19 +109,22 @@ typedef semantic_map_load_utilties::DynamicObjectData ObjectData; using namespace std; using namespace semantic_map_load_utilties; -std::string overall_folder = "~/.semanticMap/"; +std::string overall_folder = ""; boost::shared_ptr viewer; -int visualization_lvl = 0; -std::string outtopic = "/some/topic"; - -std::string modelouttopic = "/model/out/topic"; +int visualization_lvl = 0; +std::string outtopic = ""; +std::string modelouttopic = ""; ros::Publisher out_pub; ros::Publisher model_pub; + +std::vector< ros::Publisher > m_PublisherStatuss; +std::vector< ros::Publisher > out_pubs; +std::vector< ros::Publisher > model_pubs; + + std::string posepath = "testposes.xml"; std::vector sweepPoses; -#include -#include bool testDynamicObjectServiceCallback(std::string path); bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res); @@ -643,10 +653,13 @@ std::vector readPoseXML(std::string xmlFile){ return poses; } -void processMetaroom(std::string path){ +int processMetaroom(std::string path){ + int returnval = 0; printf("processing: %s\n",path.c_str()); - testDynamicObjectServiceCallback(path); - exit(0); + // testDynamicObjectServiceCallback(path); + // exit(0); + + if ( ! boost::filesystem::exists( path ) ){return 0;} int slash_pos = path.find_last_of("/"); std::string sweep_folder = path.substr(0, slash_pos) + "/"; @@ -704,9 +717,14 @@ void processMetaroom(std::string path){ quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization_lvl > 0); - remove_old_seg(sweep_folder); + if(models.size() == 0){ + returnval = 2; + }else{ + returnval = 3; + } + for(unsigned int i = 0; i < models.size(); i++){ std::vector internal_masks = internal[i]; std::vector external_masks = external[i]; @@ -912,7 +930,6 @@ void processMetaroom(std::string path){ xmlWriter->writeAttribute("z", QString::number(sumz/sum)); xmlWriter->writeEndElement(); - for(unsigned int j = 0; j < masks.size(); j++){ char buf [1024]; sprintf(buf,"%s/movingmask_%i_%i.png",sweep_folder.c_str(),movingCounter-1,imgnr[j]); @@ -931,256 +948,13 @@ void processMetaroom(std::string path){ movingCounter++; }else{break;} } - /* - return; - exit(0); - - std::vector dynamic_frameid; - std::vector dynamic_pixelid; - - std::vector moving_frameid; - std::vector moving_pixelid; - - pcl::PointCloud::Ptr dynamiccloud (new pcl::PointCloud); - pcl::PointCloud::Ptr movingcloud (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - - for(unsigned int j = 0; j < mod_fr.size(); j++){ - reglib::RGBDFrame * frame = mod_fr[j]; - std::cout << mod_po[j] << std::endl; - Eigen::Matrix4d p = mod_po[j]*bg->frames.front()->pose; - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); - - reglib::Camera * camera = frame->camera; - - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); - unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); - unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - pcl::PointXYZRGBNormal point; - point.x = m00*x + m01*y + m02*z + m03; - point.y = m10*x + m11*y + m12*z + m13; - point.z = m20*x + m21*y + m22*z + m23; - - point.b = rgbdata[3*ind+0]; - point.g = rgbdata[3*ind+1]; - point.r = rgbdata[3*ind+2]; - - if(dynamicmaskdata[ind] != 0){ - dynamiccloud->points.push_back(point); - dynamic_frameid.push_back(j); - dynamic_pixelid.push_back(ind); - point.b = 0; - point.g = 255; - point.r = 0; - }else if(internalmaskdata[ind] == 0){ - movingcloud->points.push_back(point); - moving_frameid.push_back(j); - moving_pixelid.push_back(ind); - point.b = 0; - point.g = 0; - point.r = 255; - } - - cloud->points.push_back(point); - } - } - } - } - - - // if(visualization_lvl > 0 && cloud->points.size() > 0){ - // viewer->removeAllPointClouds(); - // viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "scloud"); - // viewer->spin(); - // } - - printf("dynamiccloud: %i\n",int(dynamiccloud->points.size())); - if(dynamiccloud->points.size() > 0){ - // Creating the KdTree object for the search method of the extraction - pcl::search::KdTree::Ptr dynamictree (new pcl::search::KdTree); - dynamictree->setInputCloud (dynamiccloud); - - std::vector dynamic_indices; - pcl::EuclideanClusterExtraction dynamic_ec; - dynamic_ec.setClusterTolerance (0.02); // 2cm - dynamic_ec.setMinClusterSize (500); - dynamic_ec.setMaxClusterSize (250000000); - dynamic_ec.setSearchMethod (dynamictree); - dynamic_ec.setInputCloud (dynamiccloud); - dynamic_ec.extract (dynamic_indices); - - for (unsigned int d = 0; d < dynamic_indices.size(); d++){ - std::vector< std::vector > inds; - inds.resize(mod_fr.size()); - - double sumx = 0; - double sumy = 0; - double sumz = 0; - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ - int pid = dynamic_indices[d].indices[ind]; - inds[dynamic_frameid[pid]].push_back(dynamic_pixelid[pid]); - cloud_cluster->points.push_back(dynamiccloud->points[pid]); - sumx += cloud_cluster->points.back().x; - sumy += cloud_cluster->points.back().y; - sumz += cloud_cluster->points.back().z; - } - - sumx /= double(cloud_cluster->points.size()); - sumy /= double(cloud_cluster->points.size()); - sumz /= double(cloud_cluster->points.size()); - - char buf [1024]; - sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),d); - QFile file(buf); - if (file.exists()){file.remove();} - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("Object"); - xmlWriter->writeAttribute("object_number", QString::number(d)); - xmlWriter->writeStartElement("Mean"); - xmlWriter->writeAttribute("x", QString::number(sumx)); - xmlWriter->writeAttribute("y", QString::number(sumy)); - xmlWriter->writeAttribute("z", QString::number(sumz)); - xmlWriter->writeEndElement(); - - // token = xmlReader->readNext(); - // double x = atof(xmlReader->readElementText().toStdString().c_str()); - - // token = xmlReader->readNext(); - // double y = atof(xmlReader->readElementText().toStdString().c_str()); - - // token = xmlReader->readNext(); - // double z = atof(xmlReader->readElementText().toStdString().c_str()); - - // printf("mean: %f %f %f\n",x,y,z); - - for(unsigned int j = 0; j < mod_fr.size(); j++){ - if(inds[j].size() == 0){continue;} - reglib::RGBDFrame * frame = mod_fr[j]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 0;} - for(unsigned int k = 0; k < inds[j].size(); k++){maskdata[inds[j][k]] = 255;} - - char buf [1024]; - sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),d,j); - cv::imwrite(buf, mask ); - - sprintf(buf,"dynamicmask_%i_%i.png",d,j); - xmlWriter->writeStartElement("Mask"); - xmlWriter->writeAttribute("filename", QString(buf)); - xmlWriter->writeAttribute("image_number", QString::number(j)); - xmlWriter->writeEndElement(); - } - - xmlWriter->writeEndElement(); - xmlWriter->writeEndDocument(); - delete xmlWriter; - } - } - - - printf("movingcloud: %i\n",int(movingcloud->points.size())); - if(movingcloud->points.size() > 0){ - // Creating the KdTree object for the search method of the extraction - pcl::search::KdTree::Ptr movingtree (new pcl::search::KdTree); - movingtree->setInputCloud (movingcloud); - - std::vector moving_indices; - pcl::EuclideanClusterExtraction moving_ec; - moving_ec.setClusterTolerance (0.02); // 2cm - moving_ec.setMinClusterSize (500); - moving_ec.setMaxClusterSize (250000000); - moving_ec.setSearchMethod (movingtree); - moving_ec.setInputCloud (movingcloud); - moving_ec.extract (moving_indices); - - for (unsigned int d = 0; d < moving_indices.size(); d++){ - std::vector< std::vector > inds; - inds.resize(mod_fr.size()); - - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ - int pid = moving_indices[d].indices[ind]; - inds[moving_frameid[pid]].push_back(moving_pixelid[pid]); - cloud_cluster->points.push_back(movingcloud->points[pid]); - } - - char buf [1024]; - sprintf(buf,"%s/moving_object%10.10i.xml",sweep_folder.c_str(),d); - QFile file(buf); - if (file.exists()){file.remove();} - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save moving object as XML"<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("Object"); - xmlWriter->writeAttribute("object_number", QString::number(d)); - - for(unsigned int j = 0; j < mod_fr.size(); j++){ - if(inds[j].size() == 0){continue;} - reglib::RGBDFrame * frame = mod_fr[j]; - reglib::Camera * cam = frame->camera; - cv::Mat mask; - mask.create(cam->height,cam->width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 0;} - for(unsigned int k = 0; k < inds[j].size(); k++){maskdata[inds[j][k]] = 255;} - - char buf [1024]; - sprintf(buf,"%s/movingmask_%i_%i.png",sweep_folder.c_str(),d,j); - cv::imwrite(buf, mask ); - xmlWriter->writeStartElement("Mask"); - xmlWriter->writeAttribute("filename", QString(buf)); - xmlWriter->writeAttribute("image_number", QString::number(j)); - xmlWriter->writeEndElement(); - } - - xmlWriter->writeEndElement(); - xmlWriter->writeEndDocument(); - delete xmlWriter; - } - - } - */ } bg->fullDelete(); delete bg; + }else{ + returnval = 1; } - fullmodel->fullDelete(); delete fullmodel; // delete bgmassreg; @@ -1190,14 +964,13 @@ void processMetaroom(std::string path){ std_msgs::String msg; msg.data = path; - out_pub.publish(msg); + for(unsigned int i = 0; i < out_pubs.size(); i++){out_pubs[i].publish(msg);} ros::spinOnce(); -} -void chatterCallback(const std_msgs::String::ConstPtr& msg){ - processMetaroom(msg->data); + return returnval; } +void chatterCallback(const std_msgs::String::ConstPtr& msg){processMetaroom(msg->data);} void trainMetaroom(std::string path){ printf("processing: %s\n",path.c_str()); @@ -1259,25 +1032,20 @@ std::vector loadModels(std::string path){ reglib::Model * mod = new reglib::Model(); QXmlStreamReader* xmlReader = new QXmlStreamReader(&file); - while (!xmlReader->atEnd() && !xmlReader->hasError()) - { + while (!xmlReader->atEnd() && !xmlReader->hasError()){ QXmlStreamReader::TokenType token = xmlReader->readNext(); if (token == QXmlStreamReader::StartDocument) continue; - if (xmlReader->hasError()) - { + if (xmlReader->hasError()){ ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); break; } QString elementName = xmlReader->name().toString(); - if (token == QXmlStreamReader::StartElement) - { - - if (xmlReader->name() == "Mask") - { + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "Mask"){ int number = 0; cv::Mat mask; QXmlStreamAttributes attributes = xmlReader->attributes(); @@ -1289,8 +1057,7 @@ std::vector loadModels(std::string path){ }else{break;} - if (attributes.hasAttribute("image_number")) - { + if (attributes.hasAttribute("image_number")){ QString depthpath = attributes.value("image_number").toString(); number = atoi(depthpath.toStdString().c_str()); printf("number: %i\n",number); @@ -1314,7 +1081,7 @@ std::vector loadModels(std::string path){ void addModelToModelServer(reglib::Model * model){ printf("addModelToModelServer\n"); - model_pub.publish(quasimodo_brain::getModelMSG(model)); + for(unsigned int i = 0; i < model_pubs.size(); i++){model_pubs[i].publish(quasimodo_brain::getModelMSG(model));} ros::spinOnce(); } @@ -1328,15 +1095,7 @@ void sendMetaroomToServer(std::string path){ } -void sendCallback(const std_msgs::String::ConstPtr& msg){ - sendMetaroomToServer(msg->data); -} - -//string waypoint_id -//--- -//string[] object_id -//sensor_msgs/PointCloud2[] objects -//geometry_msgs/Point[] centroids +void sendCallback(const std_msgs::String::ConstPtr& msg){sendMetaroomToServer(msg->data);} bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res){ printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n"); @@ -1399,7 +1158,7 @@ bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDyn // req.waypoint_id = path; // DynamicObjectsServiceResponse res; // return dynamicObjectsServiceCallback(req,res); - return true; + return false; } @@ -1412,33 +1171,59 @@ bool testDynamicObjectServiceCallback(std::string path){ return dynamicObjectsServiceCallback(req,res); } +void roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& obs_msg) { + std::cout<<"Room obs message received"<xml_file_name); -int main(int argc, char** argv){ + std_msgs::String msg; + if(ret == 0){ + ROS_ERROR_STREAM("Xml file does not exist. Aborting."); + msg.data = "error_processing_observation"; + }else{msg.data = "finished_processing_observation";} - bool once = false; + if(ret == 1){ROS_ERROR_STREAM("First metaroom.");} + if(ret == 2){ROS_ERROR_STREAM("No moving objects found.");} + if(ret == 3){ROS_ERROR_STREAM("Moving objects found.");} + + for(unsigned int i = 0; i < m_PublisherStatuss.size(); i++){m_PublisherStatuss[i].publish(msg);} +} + + +void setLargeStack(){ const rlim_t kStackSize = 256 * 1024 * 1024; // min stack size = 256 MB struct rlimit rl; unsigned long result; result = getrlimit(RLIMIT_STACK, &rl); - //printf("result: %ld mb and %ld mb\n",rl.rlim_cur/(1024*1024),rl.rlim_max/(1024*1024)); if (result == 0){ if (rl.rlim_cur < kStackSize){ rl.rlim_cur = kStackSize; result = setrlimit(RLIMIT_STACK, &rl); - if (result != 0){ - fprintf(stderr, "setrlimit returned result = %d\n", int(result)); - } - //printf("result: %ld mb and %ld mb\n",rl.rlim_cur/(1024*1024),rl.rlim_max/(1024*1024)); + if (result != 0){fprintf(stderr, "setrlimit returned result = %d\n", int(result));} } } +} +int main(int argc, char** argv){ + + bool baseSetting = true; + bool once = false; + + overall_folder = std::string(getenv ("HOME"))+"/.semanticMap/"; + outtopic = "/some/topic"; + modelouttopic = "/model/out/topic"; + posepath = "testposes.xml"; ros::init(argc, argv, "metaroom_additional_view_processing"); ros::NodeHandle n; std::vector segsubs; std::vector sendsubs; + std::vector roomObservationSubs; + + std::vector trainMetarooms; + std::vector sendMetaroomToServers; + std::vector processMetarooms; int inputstate = 0; for(int i = 1; i < argc;i++){ @@ -1454,55 +1239,92 @@ int main(int argc, char** argv){ visualization_lvl = 1; inputstate = 3; } - else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 4;} - else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} - else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} - else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} - else if(std::string(argv[i]).compare("-sendModel") == 0){ inputstate = 8;} - else if(std::string(argv[i]).compare("-sendSub") == 0) { inputstate = 9;} - else if(std::string(argv[i]).compare("-sendTopic") == 0){ inputstate = 10;} - else if(std::string(argv[i]).compare("-once") == 0){ once = true;} + else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 4;} + else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} + else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} + else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} + else if(std::string(argv[i]).compare("-sendModel") == 0){ inputstate = 8;} + else if(std::string(argv[i]).compare("-sendSub") == 0) { inputstate = 9;} + else if(std::string(argv[i]).compare("-sendTopic") == 0){ inputstate = 10;} + else if(std::string(argv[i]).compare("-roomObservationTopic") == 0){ inputstate = 11;} + else if(std::string(argv[i]).compare("-DynamicObjectsService") == 0){ inputstate = 12;} + else if(std::string(argv[i]).compare("-GetDynamicObjectService") == 0){ inputstate = 13;} + else if(std::string(argv[i]).compare("-statusmsg") == 0){ inputstate = 14;} + else if(std::string(argv[i]).compare("-once") == 0){ once = true;} + else if(std::string(argv[i]).compare("-nobase") == 0){ baseSetting = false;} else if(inputstate == 0){ - if(sendsubs.size() == 0){ - model_pub = n.advertise(modelouttopic, 1000); - sendsubs.push_back(n.subscribe(std::string(argv[i]), 1000, sendCallback)); - } - out_pub = n.advertise(outtopic, 1000); segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); }else if(inputstate == 1){ - outtopic = std::string(argv[i]); + out_pubs.push_back(n.advertise(std::string(argv[i]), 1000)); }else if(inputstate == 2){ - out_pub = n.advertise(outtopic, 1000); - if(sendsubs.size() == 0){ - printf("creating model pub: %s\n",modelouttopic.c_str()); - model_pub = n.advertise(modelouttopic, 1000); - printf("creating sendsubs %s\n",modelouttopic.c_str()); - sendsubs.push_back(n.subscribe(outtopic, 1000, sendCallback)); - } - processMetaroom(std::string(argv[i])); + processMetarooms.push_back(std::string(argv[i])); }else if(inputstate == 3){ visualization_lvl = atoi(argv[i]); }else if(inputstate == 4){ overall_folder = std::string(argv[i]); }else if(inputstate == 5){ - trainMetaroom(std::string(argv[i])); + trainMetarooms.push_back(std::string(argv[i])); }else if(inputstate == 6){ posepath = std::string(argv[i]); }else if(inputstate == 7){ sweepPoses = readPoseXML(std::string(argv[i])); }else if(inputstate == 8){ - model_pub = n.advertise(modelouttopic, 1000); - sendMetaroomToServer(std::string(argv[i])); + sendMetaroomToServers.push_back(std::string(argv[i])); }else if(inputstate == 9){ - model_pub = n.advertise(modelouttopic, 1000); sendsubs.push_back(n.subscribe(std::string(argv[i]), 1000, sendCallback)); }else if(inputstate == 10){ - modelouttopic = std::string(argv[i]); + model_pubs.push_back(n.advertise(std::string(argv[i]), 1000)); + }else if(inputstate == 11){ + roomObservationSubs.push_back(n.subscribe(std::string(argv[i]), 1000, roomObservationCallback)); + }else if(inputstate == 12){ + m_DynamicObjectsServiceServers.push_back(n.advertiseService(std::string(argv[i]), dynamicObjectsServiceCallback)); + }else if(inputstate == 13){ + m_GetDynamicObjectServiceServers.push_back(n.advertiseService(std::string(argv[i]), getDynamicObjectServiceCallback)); + }else if(inputstate == 14){ + m_PublisherStatuss.push_back(n.advertise(std::string(argv[i]), 1000)); } } - m_DynamicObjectsServiceServer = n.advertiseService("/object_manager_node/ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback); - m_GetDynamicObjectServiceServer = n.advertiseService("/object_manager_node/ObjectManager/GetDynamicObjectService", getDynamicObjectServiceCallback); + if(baseSetting){ + if(m_PublisherStatuss.size() == 0){ + m_PublisherStatuss.push_back(n.advertise("/local_metric_map/status", 1000)); + } + + if(segsubs.size() == 0){ + segsubs.push_back(n.subscribe("/some/inputtopic", 1000, chatterCallback)); + } + + if(out_pubs.size() == 0){ + out_pubs.push_back(n.advertise("/some/topic", 1000)); + } + + if(model_pubs.size() == 0){ + model_pubs.push_back(n.advertise("/model/out/topic", 1000)); + } + + if(sendsubs.size() == 0){ + sendsubs.push_back(n.subscribe("/model/out/topic/path", 1000, sendCallback)); + } + + if(roomObservationSubs.size() == 0){ + roomObservationSubs.push_back(n.subscribe("/local_metric_map/room_observations", 1000, roomObservationCallback)); + } + + if(m_DynamicObjectsServiceServers.size() == 0){ + m_DynamicObjectsServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback)); + } + + if(m_GetDynamicObjectServiceServers.size() == 0){ + m_GetDynamicObjectServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/GetDynamicObjectService", getDynamicObjectServiceCallback)); + } + } + + printf("overall_folder: %s\n",overall_folder.c_str()); + + for(unsigned int i = 0; i < trainMetarooms.size(); i++){ trainMetaroom( trainMetarooms[i]);} + for(unsigned int i = 0; i < processMetarooms.size(); i++){ processMetaroom( processMetarooms[i]);} + for(unsigned int i = 0; i < sendMetaroomToServers.size(); i++){ sendMetaroomToServer( sendMetaroomToServers[i]);} + if(!once){ros::spin();} printf("done...\n"); return 0; From b7ce668285f956dab52812286276c851a0c2f0da Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 18:30:43 +0200 Subject: [PATCH 129/255] ... --- learn_objects_action/launch/learn_objects_dependencies.launch | 4 ++-- semantic_map/launch/semantic_map.launch | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/learn_objects_action/launch/learn_objects_dependencies.launch b/learn_objects_action/launch/learn_objects_dependencies.launch index fee956c6..7bae7b07 100644 --- a/learn_objects_action/launch/learn_objects_dependencies.launch +++ b/learn_objects_action/launch/learn_objects_dependencies.launch @@ -16,9 +16,9 @@ - + diff --git a/semantic_map/launch/semantic_map.launch b/semantic_map/launch/semantic_map.launch index 7b27d1bc..1c14c05c 100644 --- a/semantic_map/launch/semantic_map.launch +++ b/semantic_map/launch/semantic_map.launch @@ -11,7 +11,7 @@ - + - +--> From bc8184fd7561f010ebd1dc6409992aac616bd96b Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 18:44:56 +0200 Subject: [PATCH 130/255] ... --- semantic_map/launch/semantic_map.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/semantic_map/launch/semantic_map.launch b/semantic_map/launch/semantic_map.launch index 1c14c05c..c14f5a5e 100644 --- a/semantic_map/launch/semantic_map.launch +++ b/semantic_map/launch/semantic_map.launch @@ -11,7 +11,7 @@ - ---> + diff --git a/semantic_map_launcher/launch/semantic_map.launch b/semantic_map_launcher/launch/semantic_map.launch index 8ff05e42..ecd03d30 100644 --- a/semantic_map_launcher/launch/semantic_map.launch +++ b/semantic_map_launcher/launch/semantic_map.launch @@ -38,7 +38,7 @@ - + - +--> From 7a9a6276a9d482294469c1aacfd16b41e8008d7f Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 20:03:09 +0200 Subject: [PATCH 132/255] ... --- .../launch/semantic_map.launch | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/semantic_map_launcher/launch/semantic_map.launch b/semantic_map_launcher/launch/semantic_map.launch index ecd03d30..7a2e6627 100644 --- a/semantic_map_launcher/launch/semantic_map.launch +++ b/semantic_map_launcher/launch/semantic_map.launch @@ -38,20 +38,23 @@ - - ---> + + + + From fd877a0ae4581054f13134e79d481a2154cb6e1b Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 20:06:43 +0200 Subject: [PATCH 133/255] ... --- semantic_map_launcher/launch/semantic_map.launch | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/semantic_map_launcher/launch/semantic_map.launch b/semantic_map_launcher/launch/semantic_map.launch index 7a2e6627..916cf3d0 100644 --- a/semantic_map_launcher/launch/semantic_map.launch +++ b/semantic_map_launcher/launch/semantic_map.launch @@ -39,13 +39,6 @@ - - - - - + + + + - - - - - - - - - From fc27c1279e3452b6c26b2a60e66c4db82e7b4fd9 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 20:39:22 +0200 Subject: [PATCH 135/255] ... --- .../src/metaroom_additional_view_processing.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index a44f4500..957c5122 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1099,7 +1099,22 @@ void sendCallback(const std_msgs::String::ConstPtr& msg){sendMetaroomToServer(ms bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res){ printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n"); - std::string path = req.waypoint_id; + std::string current_waypointid = req.waypoint_id; + + + if(overall_folder.back() == '/'){overall_folder.pop_back();} + + int prevind = -1; + std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + for (unsigned int i = 0; i < sweep_xmls.size(); i++){ + SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); + std::string other_waypointid = other_roomData.roomWaypointId; + + //if(sweep_xmls[i].compare(path) == 0){break;} + if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} + } + if(prevind == -1){return false;} + std::string path = sweep_xmls[prevind]; //processMetaroom(path); printf("path: %s\n",path.c_str()); From fa474fce6cb3bc045c2a9db4e5e22fccf9bc4bf1 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 20:44:00 +0200 Subject: [PATCH 136/255] ... --- .../quasimodo_brain/src/metaroom_additional_view_processing.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 957c5122..7051986c 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1104,6 +1104,7 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj if(overall_folder.back() == '/'){overall_folder.pop_back();} + SimpleXMLParser parser; int prevind = -1; std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); for (unsigned int i = 0; i < sweep_xmls.size(); i++){ From ae5ccdb8bf1adfb1919d88cbdb5c4eab061df57f Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 20:58:28 +0200 Subject: [PATCH 137/255] ... --- .../metaroom_additional_view_processing.cpp | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 7051986c..c7e4e85d 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1097,6 +1097,13 @@ void sendMetaroomToServer(std::string path){ void sendCallback(const std_msgs::String::ConstPtr& msg){sendMetaroomToServer(msg->data);} +//string waypoint_id +//--- +//string[] object_id +//sensor_msgs/PointCloud2[] objects +//geometry_msgs/Point[] centroids + + bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res){ printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n"); std::string current_waypointid = req.waypoint_id; @@ -1110,8 +1117,6 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj for (unsigned int i = 0; i < sweep_xmls.size(); i++){ SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); std::string other_waypointid = other_roomData.roomWaypointId; - - //if(sweep_xmls[i].compare(path) == 0){break;} if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} } if(prevind == -1){return false;} @@ -1124,7 +1129,6 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj printf("sweep_folder: %s\n",sweep_folder.c_str()); - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_object*.xml")); for (auto objectFile : objectFiles){ std::string object = sweep_folder+objectFile.toStdString(); @@ -1160,6 +1164,15 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj double y = atof(attributes.value("y").toString().toStdString().c_str()); double z = atof(attributes.value("z").toString().toStdString().c_str()); printf("mean: %f %f %f\n",x,y,z); + + res.object_id.push_back(object); + sensor_msgs::PointCloud2 pc; + res.objects.push_back(pc); + geometry_msgs::Point p; + p.x = x; + p.y = y; + p.z = z; + res.centroids.push_back(p); } } } From a7caf3a36c63e7dc7a1b389c97489591d3495100 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 21:18:52 +0200 Subject: [PATCH 138/255] ... --- .../src/learn_objects_action/metric_sweep.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index bd349195..c1ab4f8e 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -127,6 +127,7 @@ def __init__(self, rois_file, debug_mode, debug_services): def execute(self, userdata): + print "SelectCluster::execute" try: # Load the waypoint to soma from file if self._rois_file != "NONE": @@ -139,6 +140,9 @@ def execute(self, userdata): #clusters = self._get_clusters(userdata['waypoint']) clusters = self._get_clusters(userdata.action_goal.waypoint) + + print "Line 1" + if self._debug_mode: available=[str(s) for s in range(len(clusters.object_id))] self._debug_services.set_status("SELECT CLUSTER: ["+",".join(available)+"]") @@ -146,6 +150,8 @@ def execute(self, userdata): else: select="auto" + print "Line 2" + if select=="PREEMPT" or select=="none": return 'none' elif select=="auto": @@ -169,6 +175,9 @@ def execute(self, userdata): else: ID=int(select) + + print "Line 3" + rospy.logwarn( "Getting cluster: %s"%clusters.object_id[ID]) #one_cluster = self._get_specific_cluster(userdata['waypoint'], clusters.object_id[ID]) one_cluster = self._get_specific_cluster(userdata.action_goal.waypoint, From 37d895b545eaee03c0e946ee14dfec0e978668fa Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 21:25:52 +0200 Subject: [PATCH 139/255] ... --- .../src/learn_objects_action/metric_sweep.py | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index c1ab4f8e..bcde0524 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -126,8 +126,7 @@ def __init__(self, rois_file, debug_mode, debug_services): self._debug_services = debug_services - def execute(self, userdata): - print "SelectCluster::execute" + def execute(self, userdata): try: # Load the waypoint to soma from file if self._rois_file != "NONE": @@ -138,10 +137,7 @@ def execute(self, userdata): soma_region = "" #clusters = self._get_clusters(userdata['waypoint']) - clusters = self._get_clusters(userdata.action_goal.waypoint) - - - print "Line 1" + clusters = self._get_clusters(userdata.action_goal.waypoint) if self._debug_mode: available=[str(s) for s in range(len(clusters.object_id))] @@ -150,8 +146,6 @@ def execute(self, userdata): else: select="auto" - print "Line 2" - if select=="PREEMPT" or select=="none": return 'none' elif select=="auto": @@ -175,9 +169,6 @@ def execute(self, userdata): else: ID=int(select) - - print "Line 3" - rospy.logwarn( "Getting cluster: %s"%clusters.object_id[ID]) #one_cluster = self._get_specific_cluster(userdata['waypoint'], clusters.object_id[ID]) one_cluster = self._get_specific_cluster(userdata.action_goal.waypoint, From 32281e4920bca9d5c0728f65234cfd93bc3bb1cc Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 21:33:52 +0200 Subject: [PATCH 140/255] ... --- learn_objects_action/src/learn_objects_action/metric_sweep.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index bcde0524..bf75d7c3 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -127,7 +127,7 @@ def __init__(self, rois_file, debug_mode, debug_services): def execute(self, userdata): - try: + try: # Load the waypoint to soma from file if self._rois_file != "NONE": with open(self._rois_file, "r") as f: From caad3a4773f0866d7895796e4f774a92e4687c7f Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 21:40:10 +0200 Subject: [PATCH 141/255] ... --- .../src/learn_objects_action/metric_sweep.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index bf75d7c3..bd349195 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -126,8 +126,8 @@ def __init__(self, rois_file, debug_mode, debug_services): self._debug_services = debug_services - def execute(self, userdata): - try: + def execute(self, userdata): + try: # Load the waypoint to soma from file if self._rois_file != "NONE": with open(self._rois_file, "r") as f: @@ -137,7 +137,7 @@ def execute(self, userdata): soma_region = "" #clusters = self._get_clusters(userdata['waypoint']) - clusters = self._get_clusters(userdata.action_goal.waypoint) + clusters = self._get_clusters(userdata.action_goal.waypoint) if self._debug_mode: available=[str(s) for s in range(len(clusters.object_id))] From ef4e6e14c866ad0d8059d5ef0bc002b7d6183745 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 21:47:34 +0200 Subject: [PATCH 142/255] ... --- .../src/learn_objects_action/metric_sweep.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index bd349195..f5c8859b 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -127,7 +127,9 @@ def __init__(self, rois_file, debug_mode, debug_services): def execute(self, userdata): + print "SelectCluster::execute" try: + print "LINE::1" # Load the waypoint to soma from file if self._rois_file != "NONE": with open(self._rois_file, "r") as f: @@ -139,6 +141,8 @@ def execute(self, userdata): #clusters = self._get_clusters(userdata['waypoint']) clusters = self._get_clusters(userdata.action_goal.waypoint) + print "LINE::2" + if self._debug_mode: available=[str(s) for s in range(len(clusters.object_id))] self._debug_services.set_status("SELECT CLUSTER: ["+",".join(available)+"]") @@ -146,6 +150,8 @@ def execute(self, userdata): else: select="auto" + print "LINE::3" + if select=="PREEMPT" or select=="none": return 'none' elif select=="auto": @@ -169,6 +175,8 @@ def execute(self, userdata): else: ID=int(select) + + print "LINE:4" rospy.logwarn( "Getting cluster: %s"%clusters.object_id[ID]) #one_cluster = self._get_specific_cluster(userdata['waypoint'], clusters.object_id[ID]) one_cluster = self._get_specific_cluster(userdata.action_goal.waypoint, From 44226bd56f3e4a546ada2c9907b4d15268d9429a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 21:52:45 +0200 Subject: [PATCH 143/255] ... --- learn_objects_action/src/learn_objects_action/metric_sweep.py | 3 ++- .../src/metaroom_additional_view_processing.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index f5c8859b..e9461e20 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -137,7 +137,8 @@ def execute(self, userdata): soma_region = somas[userdata.action_goal.waypoint] else: soma_region = "" - + + print "LINE::1.5" #clusters = self._get_clusters(userdata['waypoint']) clusters = self._get_clusters(userdata.action_goal.waypoint) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index c7e4e85d..9f507f7c 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1179,7 +1179,7 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj delete xmlReader; } - return false; + return true; } bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res){ From f1ad4bf20422e874b7dee6aa91c53e6c682eafd6 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 22:42:17 +0200 Subject: [PATCH 144/255] ... --- .../src/learn_objects_action/metric_sweep.py | 7 +- .../metaroom_additional_view_processing.cpp | 46 +- ...om_additional_view_processing.cpp.autosave | 1399 +++++++++++++++++ 3 files changed, 1448 insertions(+), 4 deletions(-) create mode 100644 quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp.autosave diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index e9461e20..65788714 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -180,9 +180,10 @@ def execute(self, userdata): print "LINE:4" rospy.logwarn( "Getting cluster: %s"%clusters.object_id[ID]) #one_cluster = self._get_specific_cluster(userdata['waypoint'], clusters.object_id[ID]) - one_cluster = self._get_specific_cluster(userdata.action_goal.waypoint, - clusters.object_id[ID]) - userdata['dynamic_object']=one_cluster + + #one_cluster = self._get_specific_cluster(userdata.action_goal.waypoint, + # clusters.object_id[ID]) + #userdata['dynamic_object']=one_cluster userdata.dynamic_object_centroid = clusters.centroids[ID] userdata.dynamic_object_points = clusters.objects[ID] userdata.dynamic_object_id = clusters.object_id[ID] diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 9f507f7c..b43a5336 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1187,7 +1187,51 @@ bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDyn // req.waypoint_id = path; // DynamicObjectsServiceResponse res; // return dynamicObjectsServiceCallback(req,res); - return false; + +// string waypoint_id +// string object_id +// --- +// sensor_msgs/PointCloud2 object_cloud +// int32[] object_mask +// geometry_msgs/Transform transform_to_map +// int32 pan_angle +// int32 tilt_angle + + std::string current_waypointid = req.waypoint_id; + + + if(overall_folder.back() == '/'){overall_folder.pop_back();} + + SimpleXMLParser parser; + int prevind = -1; + std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + for (unsigned int i = 0; i < sweep_xmls.size(); i++){ + SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); + std::string other_waypointid = other_roomData.roomWaypointId; + if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} + } + if(prevind == -1){return false;} + std::string path = sweep_xmls[prevind]; + //processMetaroom(path); + + printf("path: %s\n",path.c_str()); + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + printf("sweep_folder: %s\n",sweep_folder.c_str()); + char buf [1024]; + sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),req.object_id.c_str()); + + std::string path = std::string(buf); + +// res.object_mask = object.object_indices; +// tf::transformTFToMsg(object.transform_to_map, res.transform_to_map); +// pcl::toROSMsg(*object.object_cloud, res.object_cloud); +// res.object_cloud.header.frame_id="/map"; +// res.pan_angle = -object.pan_angle; +// res.tilt_angle = -object.tilt_angle; + + return true; } diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp.autosave b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp.autosave new file mode 100644 index 00000000..618235bf --- /dev/null +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp.autosave @@ -0,0 +1,1399 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include +#include + +#include +#include + +#include "eigen_conversions/eigen_msg.h" +#include "tf_conversions/tf_eigen.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" +#include "metaroom_xml_parser/simple_summary_parser.h" + +#include + +#include "ros/ros.h" +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" +#include "quasimodo_msgs/segment_model.h" +#include "quasimodo_msgs/metaroom_pair.h" + +#include "ros/ros.h" +#include +#include +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" +#include "metaroom_xml_parser/load_utilities.h" + +#include +#include + +#include "modelupdater/ModelUpdater.h" +#include "core/RGBDFrame.h" +#include "Util/Util.h" + +// Services +#include +#include +#include + +// Registration service +#include + +// Additional view mask service +#include + + +#include +#include + +// Custom messages +#include +#include + + +#include + +std::vector< ros::ServiceServer > m_DynamicObjectsServiceServers; +std::vector< ros::ServiceServer > m_GetDynamicObjectServiceServers; + +typedef typename object_manager_msgs::DynamicObjectsService::Request DynamicObjectsServiceRequest; +typedef typename object_manager_msgs::DynamicObjectsService::Response DynamicObjectsServiceResponse; + +typedef typename object_manager_msgs::GetDynamicObjectService::Request GetDynamicObjectServiceRequest; +typedef typename object_manager_msgs::GetDynamicObjectService::Response GetDynamicObjectServiceResponse; + +typedef typename object_manager_msgs::ProcessDynamicObjectService::Request ProcessDynamicObjectServiceRequest; +typedef typename object_manager_msgs::ProcessDynamicObjectService::Response ProcessDynamicObjectServiceResponse; + +ros::ServiceClient segmentation_client; +using namespace std; +using namespace semantic_map_load_utilties; + +typedef pcl::PointXYZRGB PointType; +typedef pcl::PointCloud Cloud; +typedef typename Cloud::Ptr CloudPtr; +typedef pcl::search::KdTree Tree; +typedef semantic_map_load_utilties::DynamicObjectData ObjectData; + +using namespace std; +using namespace semantic_map_load_utilties; + +std::string overall_folder = ""; +boost::shared_ptr viewer; +int visualization_lvl = 0; +std::string outtopic = ""; +std::string modelouttopic = ""; +ros::Publisher out_pub; +ros::Publisher model_pub; + +std::vector< ros::Publisher > m_PublisherStatuss; +std::vector< ros::Publisher > out_pubs; +std::vector< ros::Publisher > model_pubs; + + +std::string posepath = "testposes.xml"; +std::vector sweepPoses; + + +bool testDynamicObjectServiceCallback(std::string path); +bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res); + +void remove_old_seg(std::string sweep_folder){ + DIR *dir; + struct dirent *ent; + if ((dir = opendir (sweep_folder.c_str())) != NULL) { + /* print all the files and directories within directory */ + while ((ent = readdir (dir)) != NULL) { + std::string file = std::string(ent->d_name); + + if (file.find("dynamic_object") !=std::string::npos && file.find(".xml") !=std::string::npos){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } + + if (file.find("dynamicmask") !=std::string::npos && file.find(".png") !=std::string::npos){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } + + if (file.find("moving_object") !=std::string::npos && file.find(".xml") !=std::string::npos){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } + + if (file.find("movingmask") !=std::string::npos && file.find(".png") !=std::string::npos){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } + } + closedir (dir); + } +} + +void writePose(QXmlStreamWriter* xmlWriter, Eigen::Matrix4d pose){ + Eigen::Quaterniond q = Eigen::Quaterniond ((Eigen::Affine3d (pose)).rotation()); + + xmlWriter->writeStartElement("Translation"); + xmlWriter->writeStartElement("x"); + xmlWriter->writeCharacters(QString::number(pose(0,3))); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("y"); + xmlWriter->writeCharacters(QString::number(pose(1,3))); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("z"); + xmlWriter->writeCharacters(QString::number(pose(2,3))); + xmlWriter->writeEndElement(); + xmlWriter->writeEndElement(); // Translation + + xmlWriter->writeStartElement("Rotation"); + xmlWriter->writeStartElement("w"); + xmlWriter->writeCharacters(QString::number(q.w())); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("x"); + xmlWriter->writeCharacters(QString::number(q.x())); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("y"); + xmlWriter->writeCharacters(QString::number(q.y())); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("z"); + xmlWriter->writeCharacters(QString::number(q.z())); + xmlWriter->writeEndElement(); + xmlWriter->writeEndElement(); //Rotation +} + +void writeXml(std::string xmlFile, std::vector & frames, std::vector & poses){ + int slash_pos = xmlFile.find_last_of("/"); + std::string sweep_folder = xmlFile.substr(0, slash_pos) + "/"; + + QFile file(xmlFile.c_str()); + if (file.exists()){file.remove();} + + + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)) + { + std::cerr<<"Could not open file "<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Views"); + xmlWriter->writeAttribute("number_of_views", QString::number(frames.size())); + for(unsigned int i = 0; i < frames.size(); i++){ + reglib::RGBDFrame * frame = frames[i]; + char buf [1024]; + + xmlWriter->writeStartElement("View"); + sprintf(buf,"%s/view_RGB%10.10i.png",sweep_folder.c_str(),i); + cv::imwrite(buf, frame->rgb ); + + sprintf(buf,"view_RGB%10.10i.png",i); + xmlWriter->writeAttribute("RGB", QString(buf)); + + + sprintf(buf,"%s/view_DEPTH%10.10i.png",sweep_folder.c_str(),i); + cv::imwrite(buf, frame->depth ); + sprintf(buf,"view_DEPTH%10.10i.png",i); + xmlWriter->writeAttribute("DEPTH", QString(buf)); + + long nsec = 1e9*((frame->capturetime)-std::floor(frame->capturetime)); + long sec = frame->capturetime; + xmlWriter->writeStartElement("Stamp"); + + xmlWriter->writeStartElement("sec"); + xmlWriter->writeCharacters(QString::number(sec)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("nsec"); + xmlWriter->writeCharacters(QString::number(nsec)); + xmlWriter->writeEndElement(); + + xmlWriter->writeEndElement(); // Stamp + + xmlWriter->writeStartElement("Camera"); + + xmlWriter->writeStartElement("fx"); + xmlWriter->writeCharacters(QString::number(frame->camera->fx)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("fy"); + xmlWriter->writeCharacters(QString::number(frame->camera->fy)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("cx"); + xmlWriter->writeCharacters(QString::number(frame->camera->cx)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("cy"); + xmlWriter->writeCharacters(QString::number(frame->camera->cy)); + xmlWriter->writeEndElement(); + + xmlWriter->writeEndElement(); // camera + + xmlWriter->writeStartElement("RegisteredPose"); + writePose(xmlWriter,poses[i]); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("Pose"); + writePose(xmlWriter,frame->pose); + xmlWriter->writeEndElement(); + + xmlWriter->writeEndElement(); + } + xmlWriter->writeEndElement(); // Semantic Room + xmlWriter->writeEndDocument(); + delete xmlWriter; +} + +Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader){ + QXmlStreamReader::TokenType token = xmlReader->readNext();//Translation + QString elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//fx + double tx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//fy + double ty = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cx + double tz = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Translation + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//Rotation + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//qw + double qw = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qx + double qx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qy + double qy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qz + double qz = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Rotation + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d regpose = (Eigen::Affine3d(Eigen::Quaterniond(qw,qx,qy,qz))).matrix(); + regpose(0,3) = tx; + regpose(1,3) = ty; + regpose(2,3) = tz; + + return regpose; +} + +void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses){ + + QFile file(xmlFile.c_str()); + + if (!file.exists()) + { + ROS_ERROR("Could not open file %s to load room.",xmlFile.c_str()); + return; + } + + QString xmlFileQS(xmlFile.c_str()); + int index = xmlFileQS.lastIndexOf('/'); + std::string roomFolder = xmlFileQS.left(index).toStdString(); + + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()) + { + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()) + { + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + return; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement) + { + + if (xmlReader->name() == "View") + { + cv::Mat rgb; + cv::Mat depth; + //printf("elementName: %s\n",elementName.toStdString().c_str()); + QXmlStreamAttributes attributes = xmlReader->attributes(); + if (attributes.hasAttribute("RGB")) + { + std::string imgpath = attributes.value("RGB").toString().toStdString(); + printf("rgb filename: %s\n",(roomFolder+"/"+imgpath).c_str()); + rgb = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); + + //QString rgbpath = attributes.value("RGB").toString(); + //rgb = cv::imread(roomFolder+"/"+(rgbpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); + //rgb = cv::imread((rgbpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + if (attributes.hasAttribute("DEPTH")) + { + std::string imgpath = attributes.value("DEPTH").toString().toStdString(); + printf("depth filename: %s\n",(roomFolder+"/"+imgpath).c_str()); + depth = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); + //QString depthpath = attributes.value("DEPTH").toString(); + //printf("depth filename: %s\n",depthpath.toStdString().c_str()); + //depth = cv::imread((roomFolder+"/"+depthpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); + //depth = cv::imread(roomFolder+"/"+(depthpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + token = xmlReader->readNext();//Stamp + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//sec + elementName = xmlReader->name().toString(); + int sec = atoi(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//nsec + elementName = xmlReader->name().toString(); + int nsec = atoi(xmlReader->readElementText().toStdString().c_str()); + token = xmlReader->readNext();//end stamp + + token = xmlReader->readNext();//Camera + elementName = xmlReader->name().toString(); + + reglib::Camera * cam = new reglib::Camera(); + + token = xmlReader->readNext();//fx + cam->fx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//fy + cam->fy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cx + cam->cx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cy + cam->cy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Camera + elementName = xmlReader->name().toString(); + + double time = double(sec)+double(nsec)/double(1e9); + + token = xmlReader->readNext();//RegisteredPose + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d regpose = getPose(xmlReader); + + token = xmlReader->readNext();//RegisteredPose + elementName = xmlReader->name().toString(); + + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d pose = getPose(xmlReader); + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, pose); + frames.push_back(frame); + poses.push_back(pose); + } + } + } + delete xmlReader; +} + +reglib::Model * processAV(std::string path){ + printf("processAV: %s\n",path.c_str()); + + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + std::vector viewrgbs; + std::vector viewdepths; + std::vector viewtfs; + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); + for (auto objectFile : objectFiles){ + auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); + for (unsigned int i=0; iheight,cloud->width,CV_8UC3); + unsigned char * rgbdata = (unsigned char *)rgb.data; + + cv::Mat depth; + depth.create(cloud->height,cloud->width,CV_16UC1); + unsigned short * depthdata = (unsigned short *)depth.data; + + unsigned int nr_data = cloud->height * cloud->width; + for(unsigned int j = 0; j < nr_data; j++){ + PointType p = cloud->points[j]; + rgbdata[3*j+0] = p.b; + rgbdata[3*j+1] = p.g; + rgbdata[3*j+2] = p.r; + depthdata[j] = short(5000.0 * p.z); + } + + viewrgbs.push_back(rgb); + viewdepths.push_back(depth); + viewtfs.push_back(object.vAdditionalViewsTransforms[i]); + } + } + + reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path); + for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ + sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; + sweep->relativeposes[i] = sweepPoses[i]; + } + + std::vector frames; + std::vector masks; + std::vector unrefined; + + std::vector both_unrefined; + both_unrefined.push_back(Eigen::Matrix4d::Identity()); + std::vector times; + for(unsigned int i = 0; i < viewrgbs.size(); i++){ + printf("additional view: %i\n",i); + geometry_msgs::TransformStamped msg; + tf::transformStampedTFToMsg(viewtfs[i], msg); + long sec = msg.header.stamp.sec; + long nsec = msg.header.stamp.nsec; + double time = double(sec)+1e-9*double(nsec); + + Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]); + + cout << m << endl << endl; + + unrefined.push_back(m); + times.push_back(time); + + cv::Mat fullmask; + fullmask.create(480,640,CV_8UC1); + unsigned char * maskdata = (unsigned char *)fullmask.data; + for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} + masks.push_back(new reglib::ModelMask(fullmask)); + + reglib::Camera * cam = sweep->frames.front()->camera->clone(); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m);//a.matrix()); + frames.push_back(frame); + + //both_unrefined.push_back(sweep->frames.front()->pose.inverse()*a.matrix()); + both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); + } + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( sweep, reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + //sweep->points = mu->getSuperPoints(sweep->relativeposes,sweep->frames,sweep->modelmasks,1,false); + sweep->recomputeModelPoints(); + + reglib::Model * fullmodel = new reglib::Model(); + fullmodel->frames = sweep->frames; + fullmodel->relativeposes = sweep->relativeposes; + fullmodel->modelmasks = sweep->modelmasks; + + if(frames.size() > 0){ + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.15); + bgmassreg->timeout = 20; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = false; + bgmassreg->visualizationLvl = 0; + bgmassreg->maskstep = 5; + bgmassreg->nomaskstep = 5; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->addModel(sweep); + bgmassreg->setData(frames,masks); + + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); + delete bgmassreg; + + for(unsigned int i = 0; i < frames.size(); i++){ + frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; + } + + for(unsigned int i = 0; i < frames.size(); i++){ + fullmodel->frames.push_back(frames[i]); + fullmodel->modelmasks.push_back(masks[i]); + fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); + } + fullmodel->recomputeModelPoints(); + }else{ + fullmodel->points = sweep->points; + } + + delete reg; + delete mu; + delete sweep; + + + + return fullmodel; +} + +void savePoses(std::string xmlFile, std::vector poses, int maxposes = -1){ + QFile file(xmlFile.c_str()); + if (file.exists()){file.remove();} + + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){ + std::cerr<<"Could not open file "<< xmlFile <<" to save views as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Poses"); + // xmlWriter->writeAttribute("number_of_poses", QString::number(poses.size())); + for(unsigned int i = 0; i < poses.size() && (maxposes == -1 || int(i) < maxposes); i++){ + printf("saving %i\n",i); + xmlWriter->writeStartElement("Pose"); + writePose(xmlWriter,poses[i]); + xmlWriter->writeEndElement(); + } + xmlWriter->writeEndElement(); // Poses + xmlWriter->writeEndDocument(); + delete xmlWriter; +} + +std::vector readPoseXML(std::string xmlFile){ + std::vector poses; + QFile file(xmlFile.c_str()); + + if (!file.exists()){ + ROS_ERROR("Could not open file %s to load poses.",xmlFile.c_str()); + return poses; + } + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()){ + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()) + { + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + return poses; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "Pose"){ + Eigen::Matrix4d pose = getPose(xmlReader); + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + std::cout << pose << std::endl << std::endl; + poses.push_back(pose); + } + } + } + delete xmlReader; + printf("done readPoseXML\n"); + return poses; +} + +int processMetaroom(std::string path){ + int returnval = 0; + printf("processing: %s\n",path.c_str()); + // testDynamicObjectServiceCallback(path); + // exit(0); + + if ( ! boost::filesystem::exists( path ) ){return 0;} + + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + reglib::Model * fullmodel = processAV(path); + + //savePoses(sweep_folder+"testposes.xml",fullmodel->relativeposes,17); + + writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( fullmodel, reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + + std::vector po; + std::vector fr; + std::vector mm; + fullmodel->getData(po, fr, mm); + //fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); + //fullmodel->recomputeModelPoints(); + + SimpleXMLParser parser; + SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); + std::string current_waypointid = current_roomData.roomWaypointId; + + if(overall_folder.back() == '/'){overall_folder.pop_back();} + + int prevind = -1; + std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + for (unsigned int i = 0; i < sweep_xmls.size(); i++){ + SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); + std::string other_waypointid = other_roomData.roomWaypointId; + + if(sweep_xmls[i].compare(path) == 0){break;} + if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} + } + + + if(prevind != -1){ + std::string prev = sweep_xmls[prevind]; + printf("prev: %s\n",prev.c_str()); + + reglib::Model * bg = quasimodo_brain::load_metaroom_model(prev); + //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); + //bg->recomputeModelPoints(); + + std::vector< reglib::Model * > models; + models.push_back(fullmodel); + + std::vector< std::vector< cv::Mat > > internal; + std::vector< std::vector< cv::Mat > > external; + std::vector< std::vector< cv::Mat > > dynamic; + + quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization_lvl > 0); + + remove_old_seg(sweep_folder); + + if(models.size() == 0){ + returnval = 2; + }else{ + returnval = 3; + } + + for(unsigned int i = 0; i < models.size(); i++){ + std::vector internal_masks = internal[i]; + std::vector external_masks = external[i]; + std::vector dynamic_masks = dynamic[i]; + reglib::Model * model = models[i]; + + std::vector mod_po; + std::vector mod_fr; + std::vector mod_mm; + model->getData(mod_po, mod_fr, mod_mm); + + int dynamicCounter = 1; + while(true){ + double sum = 0; + double sumx = 0; + double sumy = 0; + double sumz = 0; + std::vector imgnr; + std::vector masks; + + for(unsigned int j = 0; j < mod_fr.size(); j++){ + reglib::RGBDFrame * frame = mod_fr[j]; + //std::cout << mod_po[j] << std::endl; + Eigen::Matrix4d p = mod_po[j]*bg->frames.front()->pose; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + reglib::Camera * camera = frame->camera; + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); + unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + cv::Mat mask; + mask.create(height,width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + + + bool containsData = false; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + + if(dynamicmaskdata[ind] == dynamicCounter){ + maskdata[ind] = 255; + containsData = true; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + sumx += m00*x + m01*y + m02*z + m03; + sumy += m10*x + m11*y + m12*z + m13; + sumz += m20*x + m21*y + m22*z + m23; + sum ++; + } + }else{ + maskdata[ind] = 0; + } + } + } + + if(containsData){ + masks.push_back(mask); + imgnr.push_back(j); + } + } + if(masks.size() > 0){ + char buf [1024]; + sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),dynamicCounter-1); + QFile file(buf); + if (file.exists()){file.remove();} + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Object"); + xmlWriter->writeAttribute("object_number", QString::number(dynamicCounter-1)); + xmlWriter->writeStartElement("Mean"); + xmlWriter->writeAttribute("x", QString::number(sumx/sum)); + xmlWriter->writeAttribute("y", QString::number(sumy/sum)); + xmlWriter->writeAttribute("z", QString::number(sumz/sum)); + xmlWriter->writeEndElement(); + + + for(unsigned int j = 0; j < masks.size(); j++){ + char buf [1024]; + sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),dynamicCounter-1,imgnr[j]); + cv::imwrite(buf, masks[j] ); + + sprintf(buf,"dynamicmask_%i_%i.png",dynamicCounter-1,imgnr[j]); + xmlWriter->writeStartElement("Mask"); + xmlWriter->writeAttribute("filename", QString(buf)); + xmlWriter->writeAttribute("image_number", QString::number(imgnr[j])); + xmlWriter->writeEndElement(); + } + + xmlWriter->writeEndElement(); + xmlWriter->writeEndDocument(); + delete xmlWriter; + dynamicCounter++; + }else{break;} + } + + int movingCounter = 1; + while(true){ + double sum = 0; + double sumx = 0; + double sumy = 0; + double sumz = 0; + std::vector imgnr; + std::vector masks; + + for(unsigned int j = 0; j < mod_fr.size(); j++){ + reglib::RGBDFrame * frame = mod_fr[j]; + //std::cout << mod_po[j] << std::endl; + Eigen::Matrix4d p = mod_po[j]*bg->frames.front()->pose; + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); + + reglib::Camera * camera = frame->camera; + + unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); + unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); + unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + cv::Mat mask; + mask.create(height,width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + + + bool containsData = false; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + + if(internalmaskdata[ind] == movingCounter){ + maskdata[ind] = 255; + containsData = true; + float z = idepth*float(depthdata[ind]); + if(z > 0){ + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + + sumx += m00*x + m01*y + m02*z + m03; + sumy += m10*x + m11*y + m12*z + m13; + sumz += m20*x + m21*y + m22*z + m23; + sum ++; + } + }else{ + maskdata[ind] = 0; + } + } + } + + if(containsData){ + masks.push_back(mask); + imgnr.push_back(j); + } + } + if(masks.size() > 0){ + char buf [1024]; + sprintf(buf,"%s/moving_object%10.10i.xml",sweep_folder.c_str(),movingCounter-1); + QFile file(buf); + if (file.exists()){file.remove();} + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Object"); + xmlWriter->writeAttribute("object_number", QString::number(movingCounter-1)); + xmlWriter->writeStartElement("Mean"); + xmlWriter->writeAttribute("x", QString::number(sumx/sum)); + xmlWriter->writeAttribute("y", QString::number(sumy/sum)); + xmlWriter->writeAttribute("z", QString::number(sumz/sum)); + xmlWriter->writeEndElement(); + + for(unsigned int j = 0; j < masks.size(); j++){ + char buf [1024]; + sprintf(buf,"%s/movingmask_%i_%i.png",sweep_folder.c_str(),movingCounter-1,imgnr[j]); + cv::imwrite(buf, masks[j] ); + + sprintf(buf,"movingmask_%i_%i.png",movingCounter-1,imgnr[j]); + xmlWriter->writeStartElement("Mask"); + xmlWriter->writeAttribute("filename", QString(buf)); + xmlWriter->writeAttribute("image_number", QString::number(imgnr[j])); + xmlWriter->writeEndElement(); + } + + xmlWriter->writeEndElement(); + xmlWriter->writeEndDocument(); + delete xmlWriter; + movingCounter++; + }else{break;} + } + } + bg->fullDelete(); + delete bg; + }else{ + returnval = 1; + } + + fullmodel->fullDelete(); + delete fullmodel; + // delete bgmassreg; + delete reg; + delete mu; + printf("publishing file %s to %s\n",path.c_str(),outtopic.c_str()); + + std_msgs::String msg; + msg.data = path; + for(unsigned int i = 0; i < out_pubs.size(); i++){out_pubs[i].publish(msg);} + ros::spinOnce(); + + return returnval; +} + +void chatterCallback(const std_msgs::String::ConstPtr& msg){processMetaroom(msg->data);} + +void trainMetaroom(std::string path){ + printf("processing: %s\n",path.c_str()); + if(posepath.compare("")==0){ + printf("posepath not set, set before training\n"); + return ; + } + + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + reglib::Model * fullmodel = processAV(path); + + //Not needed if metaroom well calibrated + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); + bgmassreg->timeout = 600; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = true; + bgmassreg->visualizationLvl = 0; + bgmassreg->maskstep = 5; + bgmassreg->nomaskstep = 5; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); + + savePoses(overall_folder+"/"+posepath,bgmfr.poses,17); + fullmodel->fullDelete(); + delete fullmodel; + delete bgmassreg; +} + +std::vector loadModels(std::string path){ + printf("loadModels: %s\n",path.c_str()); + std::vector models; + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + std::vector frames; + std::vector poses; + readViewXML(sweep_folder+"ViewGroup.xml",frames,poses); + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_object*.xml")); + for (auto objectFile : objectFiles){ + std::string object = sweep_folder+objectFile.toStdString(); + printf("object: %s\n",object.c_str()); + + QFile file(object.c_str()); + + if (!file.exists()){ + ROS_ERROR("Could not open file %s to masks.",object.c_str()); + continue; + } + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()){ + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()){ + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + break; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "Mask"){ + int number = 0; + cv::Mat mask; + QXmlStreamAttributes attributes = xmlReader->attributes(); + if (attributes.hasAttribute("filename")){ + QString maskpath = attributes.value("filename").toString(); + //printf("mask filename: %s\n",(sweep_folder+maskpath.toStdString()).c_str()); + mask = cv::imread(sweep_folder+"/"+(maskpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); + //mask = cv::imread((maskpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + if (attributes.hasAttribute("image_number")){ + QString depthpath = attributes.value("image_number").toString(); + number = atoi(depthpath.toStdString().c_str()); + printf("number: %i\n",number); + }else{break;} + + mod->frames.push_back(frames[number]->clone()); + mod->relativeposes.push_back(poses[number]); + mod->modelmasks.push_back(new reglib::ModelMask(mask)); + } + } + } + + delete xmlReader; + models.push_back(mod); + } + + for(unsigned int i = 0; i < frames.size(); i++){delete frames[i];} + + return models; +} + +void addModelToModelServer(reglib::Model * model){ + printf("addModelToModelServer\n"); + for(unsigned int i = 0; i < model_pubs.size(); i++){model_pubs[i].publish(quasimodo_brain::getModelMSG(model));} + ros::spinOnce(); +} + +void sendMetaroomToServer(std::string path){ + std::vector mods = loadModels(path); + for(unsigned int i = 0; i < mods.size(); i++){ + addModelToModelServer(mods[i]); + mods[i]->fullDelete(); + delete mods[i]; + } +} + + +void sendCallback(const std_msgs::String::ConstPtr& msg){sendMetaroomToServer(msg->data);} + +//string waypoint_id +//--- +//string[] object_id +//sensor_msgs/PointCloud2[] objects +//geometry_msgs/Point[] centroids + + +bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res){ + printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n"); + std::string current_waypointid = req.waypoint_id; + + + if(overall_folder.back() == '/'){overall_folder.pop_back();} + + SimpleXMLParser parser; + int prevind = -1; + std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + for (unsigned int i = 0; i < sweep_xmls.size(); i++){ + SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); + std::string other_waypointid = other_roomData.roomWaypointId; + if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} + } + if(prevind == -1){return false;} + std::string path = sweep_xmls[prevind]; + //processMetaroom(path); + + printf("path: %s\n",path.c_str()); + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + printf("sweep_folder: %s\n",sweep_folder.c_str()); + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_object*.xml")); + for (auto objectFile : objectFiles){ + std::string object = sweep_folder+objectFile.toStdString(); + printf("object: %s\n",object.c_str()); + + QFile file(object.c_str()); + + if (!file.exists()){ + ROS_ERROR("Could not open file %s to masks.",object.c_str()); + continue; + } + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()){ + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()){ + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + break; + } + + QString elementName = xmlReader->name().toString(); + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "Mean"){ + QXmlStreamAttributes attributes = xmlReader->attributes(); + double x = atof(attributes.value("x").toString().toStdString().c_str()); + double y = atof(attributes.value("y").toString().toStdString().c_str()); + double z = atof(attributes.value("z").toString().toStdString().c_str()); + printf("mean: %f %f %f\n",x,y,z); + + res.object_id.push_back(object); + sensor_msgs::PointCloud2 pc; + res.objects.push_back(pc); + geometry_msgs::Point p; + p.x = x; + p.y = y; + p.z = z; + res.centroids.push_back(p); + } + } + } + delete xmlReader; + } + + return true; +} + +bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res){ +// string waypoint_id +// string object_id +// --- +// sensor_msgs/PointCloud2 object_cloud +// int32[] object_mask +// geometry_msgs/Transform transform_to_map +// int32 pan_angle +// int32 tilt_angle + + std::string current_waypointid = req.waypoint_id; + + + if(overall_folder.back() == '/'){overall_folder.pop_back();} + + SimpleXMLParser parser; + int prevind = -1; + std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + for (unsigned int i = 0; i < sweep_xmls.size(); i++){ + SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); + std::string other_waypointid = other_roomData.roomWaypointId; + if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} + } + if(prevind == -1){return false;} + std::string path = sweep_xmls[prevind]; + //processMetaroom(path); + + printf("path: %s\n",path.c_str()); + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + printf("sweep_folder: %s\n",sweep_folder.c_str()); + char buf [1024]; + sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),req.object_id.c_str()); + + std::string path = std::string(buf); + +// res.object_mask = object.object_indices; +// tf::transformTFToMsg(object.transform_to_map, res.transform_to_map); +// pcl::toROSMsg(*object.object_cloud, res.object_cloud); +// res.object_cloud.header.frame_id="/map"; +// res.pan_angle = -object.pan_angle; +// res.tilt_angle = -object.tilt_angle; + + return true; +} + + + +bool testDynamicObjectServiceCallback(std::string path){ + printf("bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res)\n"); + DynamicObjectsServiceRequest req; + req.waypoint_id = path; + DynamicObjectsServiceResponse res; + return dynamicObjectsServiceCallback(req,res); +} + +void roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& obs_msg) { + std::cout<<"Room obs message received"<xml_file_name); + + std_msgs::String msg; + if(ret == 0){ + ROS_ERROR_STREAM("Xml file does not exist. Aborting."); + msg.data = "error_processing_observation"; + }else{msg.data = "finished_processing_observation";} + + if(ret == 1){ROS_ERROR_STREAM("First metaroom.");} + if(ret == 2){ROS_ERROR_STREAM("No moving objects found.");} + if(ret == 3){ROS_ERROR_STREAM("Moving objects found.");} + + for(unsigned int i = 0; i < m_PublisherStatuss.size(); i++){m_PublisherStatuss[i].publish(msg);} +} + + +void setLargeStack(){ + const rlim_t kStackSize = 256 * 1024 * 1024; // min stack size = 256 MB + struct rlimit rl; + unsigned long result; + + result = getrlimit(RLIMIT_STACK, &rl); + if (result == 0){ + if (rl.rlim_cur < kStackSize){ + rl.rlim_cur = kStackSize; + result = setrlimit(RLIMIT_STACK, &rl); + if (result != 0){fprintf(stderr, "setrlimit returned result = %d\n", int(result));} + } + } +} + +int main(int argc, char** argv){ + + bool baseSetting = true; + bool once = false; + + overall_folder = std::string(getenv ("HOME"))+"/.semanticMap/"; + outtopic = "/some/topic"; + modelouttopic = "/model/out/topic"; + posepath = "testposes.xml"; + + ros::init(argc, argv, "metaroom_additional_view_processing"); + ros::NodeHandle n; + + std::vector segsubs; + std::vector sendsubs; + std::vector roomObservationSubs; + + std::vector trainMetarooms; + std::vector sendMetaroomToServers; + std::vector processMetarooms; + + int inputstate = 0; + for(int i = 1; i < argc;i++){ + printf("input: %s\n",argv[i]); + if( std::string(argv[i]).compare("-intopic") == 0){ inputstate = 0;} + else if(std::string(argv[i]).compare("-outtopic") == 0){ inputstate = 1;} + else if(std::string(argv[i]).compare("-file") == 0){ inputstate = 2;} + else if(std::string(argv[i]).compare("-v") == 0){ + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + visualization_lvl = 1; + inputstate = 3; + } + else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 4;} + else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} + else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} + else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} + else if(std::string(argv[i]).compare("-sendModel") == 0){ inputstate = 8;} + else if(std::string(argv[i]).compare("-sendSub") == 0) { inputstate = 9;} + else if(std::string(argv[i]).compare("-sendTopic") == 0){ inputstate = 10;} + else if(std::string(argv[i]).compare("-roomObservationTopic") == 0){ inputstate = 11;} + else if(std::string(argv[i]).compare("-DynamicObjectsService") == 0){ inputstate = 12;} + else if(std::string(argv[i]).compare("-GetDynamicObjectService") == 0){ inputstate = 13;} + else if(std::string(argv[i]).compare("-statusmsg") == 0){ inputstate = 14;} + else if(std::string(argv[i]).compare("-once") == 0){ once = true;} + else if(std::string(argv[i]).compare("-nobase") == 0){ baseSetting = false;} + else if(inputstate == 0){ + segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); + }else if(inputstate == 1){ + out_pubs.push_back(n.advertise(std::string(argv[i]), 1000)); + }else if(inputstate == 2){ + processMetarooms.push_back(std::string(argv[i])); + }else if(inputstate == 3){ + visualization_lvl = atoi(argv[i]); + }else if(inputstate == 4){ + overall_folder = std::string(argv[i]); + }else if(inputstate == 5){ + trainMetarooms.push_back(std::string(argv[i])); + }else if(inputstate == 6){ + posepath = std::string(argv[i]); + }else if(inputstate == 7){ + sweepPoses = readPoseXML(std::string(argv[i])); + }else if(inputstate == 8){ + sendMetaroomToServers.push_back(std::string(argv[i])); + }else if(inputstate == 9){ + sendsubs.push_back(n.subscribe(std::string(argv[i]), 1000, sendCallback)); + }else if(inputstate == 10){ + model_pubs.push_back(n.advertise(std::string(argv[i]), 1000)); + }else if(inputstate == 11){ + roomObservationSubs.push_back(n.subscribe(std::string(argv[i]), 1000, roomObservationCallback)); + }else if(inputstate == 12){ + m_DynamicObjectsServiceServers.push_back(n.advertiseService(std::string(argv[i]), dynamicObjectsServiceCallback)); + }else if(inputstate == 13){ + m_GetDynamicObjectServiceServers.push_back(n.advertiseService(std::string(argv[i]), getDynamicObjectServiceCallback)); + }else if(inputstate == 14){ + m_PublisherStatuss.push_back(n.advertise(std::string(argv[i]), 1000)); + } + } + + if(baseSetting){ + if(m_PublisherStatuss.size() == 0){ + m_PublisherStatuss.push_back(n.advertise("/local_metric_map/status", 1000)); + } + + if(segsubs.size() == 0){ + segsubs.push_back(n.subscribe("/some/inputtopic", 1000, chatterCallback)); + } + + if(out_pubs.size() == 0){ + out_pubs.push_back(n.advertise("/some/topic", 1000)); + } + + if(model_pubs.size() == 0){ + model_pubs.push_back(n.advertise("/model/out/topic", 1000)); + } + + if(sendsubs.size() == 0){ + sendsubs.push_back(n.subscribe("/model/out/topic/path", 1000, sendCallback)); + } + + if(roomObservationSubs.size() == 0){ + roomObservationSubs.push_back(n.subscribe("/local_metric_map/room_observations", 1000, roomObservationCallback)); + } + + if(m_DynamicObjectsServiceServers.size() == 0){ + m_DynamicObjectsServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback)); + } + + if(m_GetDynamicObjectServiceServers.size() == 0){ + m_GetDynamicObjectServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/GetDynamicObjectService", getDynamicObjectServiceCallback)); + } + } + + printf("overall_folder: %s\n",overall_folder.c_str()); + + for(unsigned int i = 0; i < trainMetarooms.size(); i++){ trainMetaroom( trainMetarooms[i]);} + for(unsigned int i = 0; i < processMetarooms.size(); i++){ processMetaroom( processMetarooms[i]);} + for(unsigned int i = 0; i < sendMetaroomToServers.size(); i++){ sendMetaroomToServer( sendMetaroomToServers[i]);} + + if(!once){ros::spin();} + printf("done...\n"); + return 0; +} From b86205716a4557be3ab8ef7f007a83bf98689ae2 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 22:48:32 +0200 Subject: [PATCH 145/255] ... --- .../metaroom_additional_view_processing.cpp | 12 +- ...om_additional_view_processing.cpp.autosave | 1399 ----------------- 2 files changed, 4 insertions(+), 1407 deletions(-) delete mode 100644 quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp.autosave diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index b43a5336..89e5b68c 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1183,11 +1183,6 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj } bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res){ - // DynamicObjectsServiceRequest req; - // req.waypoint_id = path; - // DynamicObjectsServiceResponse res; - // return dynamicObjectsServiceCallback(req,res); - // string waypoint_id // string object_id // --- @@ -1219,10 +1214,11 @@ bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDyn std::string sweep_folder = path.substr(0, slash_pos) + "/"; printf("sweep_folder: %s\n",sweep_folder.c_str()); + printf("req.object_id: %s\n",req.object_id.c_str()); char buf [1024]; - sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),req.object_id.c_str()); - - std::string path = std::string(buf); + //sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),atoi(req.object_id.c_str()); +//sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),atoi(req.object_id.c_str()); + std::string fullpath = std::string(buf); // res.object_mask = object.object_indices; // tf::transformTFToMsg(object.transform_to_map, res.transform_to_map); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp.autosave b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp.autosave deleted file mode 100644 index 618235bf..00000000 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp.autosave +++ /dev/null @@ -1,1399 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" -#include -#include - -#include -#include - -#include "eigen_conversions/eigen_msg.h" -#include "tf_conversions/tf_eigen.h" - -#include "metaroom_xml_parser/simple_xml_parser.h" -#include "metaroom_xml_parser/simple_summary_parser.h" - -#include - -#include "ros/ros.h" -#include -#include -#include - -#include "metaroom_xml_parser/load_utilities.h" -#include -#include - -#include - -#include "quasimodo_msgs/model.h" -#include "quasimodo_msgs/rgbd_frame.h" -#include "quasimodo_msgs/model_from_frame.h" -#include "quasimodo_msgs/index_frame.h" -#include "quasimodo_msgs/fuse_models.h" -#include "quasimodo_msgs/get_model.h" -#include "quasimodo_msgs/segment_model.h" -#include "quasimodo_msgs/metaroom_pair.h" - -#include "ros/ros.h" -#include -#include -#include -#include -#include - -#include "metaroom_xml_parser/load_utilities.h" -#include -#include - -#include "quasimodo_msgs/model.h" -#include "quasimodo_msgs/rgbd_frame.h" -#include "quasimodo_msgs/model_from_frame.h" -#include "quasimodo_msgs/index_frame.h" -#include "quasimodo_msgs/fuse_models.h" -#include "quasimodo_msgs/get_model.h" - -#include "metaroom_xml_parser/simple_xml_parser.h" -#include "metaroom_xml_parser/load_utilities.h" - -#include -#include - -#include "modelupdater/ModelUpdater.h" -#include "core/RGBDFrame.h" -#include "Util/Util.h" - -// Services -#include -#include -#include - -// Registration service -#include - -// Additional view mask service -#include - - -#include -#include - -// Custom messages -#include -#include - - -#include - -std::vector< ros::ServiceServer > m_DynamicObjectsServiceServers; -std::vector< ros::ServiceServer > m_GetDynamicObjectServiceServers; - -typedef typename object_manager_msgs::DynamicObjectsService::Request DynamicObjectsServiceRequest; -typedef typename object_manager_msgs::DynamicObjectsService::Response DynamicObjectsServiceResponse; - -typedef typename object_manager_msgs::GetDynamicObjectService::Request GetDynamicObjectServiceRequest; -typedef typename object_manager_msgs::GetDynamicObjectService::Response GetDynamicObjectServiceResponse; - -typedef typename object_manager_msgs::ProcessDynamicObjectService::Request ProcessDynamicObjectServiceRequest; -typedef typename object_manager_msgs::ProcessDynamicObjectService::Response ProcessDynamicObjectServiceResponse; - -ros::ServiceClient segmentation_client; -using namespace std; -using namespace semantic_map_load_utilties; - -typedef pcl::PointXYZRGB PointType; -typedef pcl::PointCloud Cloud; -typedef typename Cloud::Ptr CloudPtr; -typedef pcl::search::KdTree Tree; -typedef semantic_map_load_utilties::DynamicObjectData ObjectData; - -using namespace std; -using namespace semantic_map_load_utilties; - -std::string overall_folder = ""; -boost::shared_ptr viewer; -int visualization_lvl = 0; -std::string outtopic = ""; -std::string modelouttopic = ""; -ros::Publisher out_pub; -ros::Publisher model_pub; - -std::vector< ros::Publisher > m_PublisherStatuss; -std::vector< ros::Publisher > out_pubs; -std::vector< ros::Publisher > model_pubs; - - -std::string posepath = "testposes.xml"; -std::vector sweepPoses; - - -bool testDynamicObjectServiceCallback(std::string path); -bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res); - -void remove_old_seg(std::string sweep_folder){ - DIR *dir; - struct dirent *ent; - if ((dir = opendir (sweep_folder.c_str())) != NULL) { - /* print all the files and directories within directory */ - while ((ent = readdir (dir)) != NULL) { - std::string file = std::string(ent->d_name); - - if (file.find("dynamic_object") !=std::string::npos && file.find(".xml") !=std::string::npos){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); - } - - if (file.find("dynamicmask") !=std::string::npos && file.find(".png") !=std::string::npos){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); - } - - if (file.find("moving_object") !=std::string::npos && file.find(".xml") !=std::string::npos){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); - } - - if (file.find("movingmask") !=std::string::npos && file.find(".png") !=std::string::npos){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); - } - } - closedir (dir); - } -} - -void writePose(QXmlStreamWriter* xmlWriter, Eigen::Matrix4d pose){ - Eigen::Quaterniond q = Eigen::Quaterniond ((Eigen::Affine3d (pose)).rotation()); - - xmlWriter->writeStartElement("Translation"); - xmlWriter->writeStartElement("x"); - xmlWriter->writeCharacters(QString::number(pose(0,3))); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("y"); - xmlWriter->writeCharacters(QString::number(pose(1,3))); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("z"); - xmlWriter->writeCharacters(QString::number(pose(2,3))); - xmlWriter->writeEndElement(); - xmlWriter->writeEndElement(); // Translation - - xmlWriter->writeStartElement("Rotation"); - xmlWriter->writeStartElement("w"); - xmlWriter->writeCharacters(QString::number(q.w())); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("x"); - xmlWriter->writeCharacters(QString::number(q.x())); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("y"); - xmlWriter->writeCharacters(QString::number(q.y())); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("z"); - xmlWriter->writeCharacters(QString::number(q.z())); - xmlWriter->writeEndElement(); - xmlWriter->writeEndElement(); //Rotation -} - -void writeXml(std::string xmlFile, std::vector & frames, std::vector & poses){ - int slash_pos = xmlFile.find_last_of("/"); - std::string sweep_folder = xmlFile.substr(0, slash_pos) + "/"; - - QFile file(xmlFile.c_str()); - if (file.exists()){file.remove();} - - - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)) - { - std::cerr<<"Could not open file "<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("Views"); - xmlWriter->writeAttribute("number_of_views", QString::number(frames.size())); - for(unsigned int i = 0; i < frames.size(); i++){ - reglib::RGBDFrame * frame = frames[i]; - char buf [1024]; - - xmlWriter->writeStartElement("View"); - sprintf(buf,"%s/view_RGB%10.10i.png",sweep_folder.c_str(),i); - cv::imwrite(buf, frame->rgb ); - - sprintf(buf,"view_RGB%10.10i.png",i); - xmlWriter->writeAttribute("RGB", QString(buf)); - - - sprintf(buf,"%s/view_DEPTH%10.10i.png",sweep_folder.c_str(),i); - cv::imwrite(buf, frame->depth ); - sprintf(buf,"view_DEPTH%10.10i.png",i); - xmlWriter->writeAttribute("DEPTH", QString(buf)); - - long nsec = 1e9*((frame->capturetime)-std::floor(frame->capturetime)); - long sec = frame->capturetime; - xmlWriter->writeStartElement("Stamp"); - - xmlWriter->writeStartElement("sec"); - xmlWriter->writeCharacters(QString::number(sec)); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("nsec"); - xmlWriter->writeCharacters(QString::number(nsec)); - xmlWriter->writeEndElement(); - - xmlWriter->writeEndElement(); // Stamp - - xmlWriter->writeStartElement("Camera"); - - xmlWriter->writeStartElement("fx"); - xmlWriter->writeCharacters(QString::number(frame->camera->fx)); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("fy"); - xmlWriter->writeCharacters(QString::number(frame->camera->fy)); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("cx"); - xmlWriter->writeCharacters(QString::number(frame->camera->cx)); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("cy"); - xmlWriter->writeCharacters(QString::number(frame->camera->cy)); - xmlWriter->writeEndElement(); - - xmlWriter->writeEndElement(); // camera - - xmlWriter->writeStartElement("RegisteredPose"); - writePose(xmlWriter,poses[i]); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("Pose"); - writePose(xmlWriter,frame->pose); - xmlWriter->writeEndElement(); - - xmlWriter->writeEndElement(); - } - xmlWriter->writeEndElement(); // Semantic Room - xmlWriter->writeEndDocument(); - delete xmlWriter; -} - -Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader){ - QXmlStreamReader::TokenType token = xmlReader->readNext();//Translation - QString elementName = xmlReader->name().toString(); - - token = xmlReader->readNext();//fx - double tx = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//fy - double ty = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//cx - double tz = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//Translation - elementName = xmlReader->name().toString(); - - token = xmlReader->readNext();//Rotation - elementName = xmlReader->name().toString(); - - token = xmlReader->readNext();//qw - double qw = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//qx - double qx = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//qy - double qy = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//qz - double qz = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//Rotation - elementName = xmlReader->name().toString(); - - Eigen::Matrix4d regpose = (Eigen::Affine3d(Eigen::Quaterniond(qw,qx,qy,qz))).matrix(); - regpose(0,3) = tx; - regpose(1,3) = ty; - regpose(2,3) = tz; - - return regpose; -} - -void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses){ - - QFile file(xmlFile.c_str()); - - if (!file.exists()) - { - ROS_ERROR("Could not open file %s to load room.",xmlFile.c_str()); - return; - } - - QString xmlFileQS(xmlFile.c_str()); - int index = xmlFileQS.lastIndexOf('/'); - std::string roomFolder = xmlFileQS.left(index).toStdString(); - - - file.open(QIODevice::ReadOnly); - ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()) - { - QXmlStreamReader::TokenType token = xmlReader->readNext(); - if (token == QXmlStreamReader::StartDocument) - continue; - - if (xmlReader->hasError()) - { - ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); - return; - } - - QString elementName = xmlReader->name().toString(); - - if (token == QXmlStreamReader::StartElement) - { - - if (xmlReader->name() == "View") - { - cv::Mat rgb; - cv::Mat depth; - //printf("elementName: %s\n",elementName.toStdString().c_str()); - QXmlStreamAttributes attributes = xmlReader->attributes(); - if (attributes.hasAttribute("RGB")) - { - std::string imgpath = attributes.value("RGB").toString().toStdString(); - printf("rgb filename: %s\n",(roomFolder+"/"+imgpath).c_str()); - rgb = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); - - //QString rgbpath = attributes.value("RGB").toString(); - //rgb = cv::imread(roomFolder+"/"+(rgbpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); - //rgb = cv::imread((rgbpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); - }else{break;} - - - if (attributes.hasAttribute("DEPTH")) - { - std::string imgpath = attributes.value("DEPTH").toString().toStdString(); - printf("depth filename: %s\n",(roomFolder+"/"+imgpath).c_str()); - depth = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); - //QString depthpath = attributes.value("DEPTH").toString(); - //printf("depth filename: %s\n",depthpath.toStdString().c_str()); - //depth = cv::imread((roomFolder+"/"+depthpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); - //depth = cv::imread(roomFolder+"/"+(depthpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); - }else{break;} - - - token = xmlReader->readNext();//Stamp - elementName = xmlReader->name().toString(); - - token = xmlReader->readNext();//sec - elementName = xmlReader->name().toString(); - int sec = atoi(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//nsec - elementName = xmlReader->name().toString(); - int nsec = atoi(xmlReader->readElementText().toStdString().c_str()); - token = xmlReader->readNext();//end stamp - - token = xmlReader->readNext();//Camera - elementName = xmlReader->name().toString(); - - reglib::Camera * cam = new reglib::Camera(); - - token = xmlReader->readNext();//fx - cam->fx = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//fy - cam->fy = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//cx - cam->cx = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//cy - cam->cy = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//Camera - elementName = xmlReader->name().toString(); - - double time = double(sec)+double(nsec)/double(1e9); - - token = xmlReader->readNext();//RegisteredPose - elementName = xmlReader->name().toString(); - - Eigen::Matrix4d regpose = getPose(xmlReader); - - token = xmlReader->readNext();//RegisteredPose - elementName = xmlReader->name().toString(); - - - token = xmlReader->readNext();//Pose - elementName = xmlReader->name().toString(); - - Eigen::Matrix4d pose = getPose(xmlReader); - - token = xmlReader->readNext();//Pose - elementName = xmlReader->name().toString(); - - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, pose); - frames.push_back(frame); - poses.push_back(pose); - } - } - } - delete xmlReader; -} - -reglib::Model * processAV(std::string path){ - printf("processAV: %s\n",path.c_str()); - - int slash_pos = path.find_last_of("/"); - std::string sweep_folder = path.substr(0, slash_pos) + "/"; - - std::vector viewrgbs; - std::vector viewdepths; - std::vector viewtfs; - - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); - for (auto objectFile : objectFiles){ - auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); - for (unsigned int i=0; iheight,cloud->width,CV_8UC3); - unsigned char * rgbdata = (unsigned char *)rgb.data; - - cv::Mat depth; - depth.create(cloud->height,cloud->width,CV_16UC1); - unsigned short * depthdata = (unsigned short *)depth.data; - - unsigned int nr_data = cloud->height * cloud->width; - for(unsigned int j = 0; j < nr_data; j++){ - PointType p = cloud->points[j]; - rgbdata[3*j+0] = p.b; - rgbdata[3*j+1] = p.g; - rgbdata[3*j+2] = p.r; - depthdata[j] = short(5000.0 * p.z); - } - - viewrgbs.push_back(rgb); - viewdepths.push_back(depth); - viewtfs.push_back(object.vAdditionalViewsTransforms[i]); - } - } - - reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path); - for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ - sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; - sweep->relativeposes[i] = sweepPoses[i]; - } - - std::vector frames; - std::vector masks; - std::vector unrefined; - - std::vector both_unrefined; - both_unrefined.push_back(Eigen::Matrix4d::Identity()); - std::vector times; - for(unsigned int i = 0; i < viewrgbs.size(); i++){ - printf("additional view: %i\n",i); - geometry_msgs::TransformStamped msg; - tf::transformStampedTFToMsg(viewtfs[i], msg); - long sec = msg.header.stamp.sec; - long nsec = msg.header.stamp.nsec; - double time = double(sec)+1e-9*double(nsec); - - Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]); - - cout << m << endl << endl; - - unrefined.push_back(m); - times.push_back(time); - - cv::Mat fullmask; - fullmask.create(480,640,CV_8UC1); - unsigned char * maskdata = (unsigned char *)fullmask.data; - for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} - masks.push_back(new reglib::ModelMask(fullmask)); - - reglib::Camera * cam = sweep->frames.front()->camera->clone(); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m);//a.matrix()); - frames.push_back(frame); - - //both_unrefined.push_back(sweep->frames.front()->pose.inverse()*a.matrix()); - both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); - } - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( sweep, reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; - - //sweep->points = mu->getSuperPoints(sweep->relativeposes,sweep->frames,sweep->modelmasks,1,false); - sweep->recomputeModelPoints(); - - reglib::Model * fullmodel = new reglib::Model(); - fullmodel->frames = sweep->frames; - fullmodel->relativeposes = sweep->relativeposes; - fullmodel->modelmasks = sweep->modelmasks; - - if(frames.size() > 0){ - reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.15); - bgmassreg->timeout = 20; - bgmassreg->viewer = viewer; - bgmassreg->use_surface = true; - bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = 0; - bgmassreg->maskstep = 5; - bgmassreg->nomaskstep = 5; - bgmassreg->nomask = true; - bgmassreg->stopval = 0.0005; - bgmassreg->addModel(sweep); - bgmassreg->setData(frames,masks); - - reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); - delete bgmassreg; - - for(unsigned int i = 0; i < frames.size(); i++){ - frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; - } - - for(unsigned int i = 0; i < frames.size(); i++){ - fullmodel->frames.push_back(frames[i]); - fullmodel->modelmasks.push_back(masks[i]); - fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); - } - fullmodel->recomputeModelPoints(); - }else{ - fullmodel->points = sweep->points; - } - - delete reg; - delete mu; - delete sweep; - - - - return fullmodel; -} - -void savePoses(std::string xmlFile, std::vector poses, int maxposes = -1){ - QFile file(xmlFile.c_str()); - if (file.exists()){file.remove();} - - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){ - std::cerr<<"Could not open file "<< xmlFile <<" to save views as XML"<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("Poses"); - // xmlWriter->writeAttribute("number_of_poses", QString::number(poses.size())); - for(unsigned int i = 0; i < poses.size() && (maxposes == -1 || int(i) < maxposes); i++){ - printf("saving %i\n",i); - xmlWriter->writeStartElement("Pose"); - writePose(xmlWriter,poses[i]); - xmlWriter->writeEndElement(); - } - xmlWriter->writeEndElement(); // Poses - xmlWriter->writeEndDocument(); - delete xmlWriter; -} - -std::vector readPoseXML(std::string xmlFile){ - std::vector poses; - QFile file(xmlFile.c_str()); - - if (!file.exists()){ - ROS_ERROR("Could not open file %s to load poses.",xmlFile.c_str()); - return poses; - } - - file.open(QIODevice::ReadOnly); - ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()){ - QXmlStreamReader::TokenType token = xmlReader->readNext(); - if (token == QXmlStreamReader::StartDocument) - continue; - - if (xmlReader->hasError()) - { - ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); - return poses; - } - - QString elementName = xmlReader->name().toString(); - - if (token == QXmlStreamReader::StartElement){ - if (xmlReader->name() == "Pose"){ - Eigen::Matrix4d pose = getPose(xmlReader); - - token = xmlReader->readNext();//Pose - elementName = xmlReader->name().toString(); - - std::cout << pose << std::endl << std::endl; - poses.push_back(pose); - } - } - } - delete xmlReader; - printf("done readPoseXML\n"); - return poses; -} - -int processMetaroom(std::string path){ - int returnval = 0; - printf("processing: %s\n",path.c_str()); - // testDynamicObjectServiceCallback(path); - // exit(0); - - if ( ! boost::filesystem::exists( path ) ){return 0;} - - int slash_pos = path.find_last_of("/"); - std::string sweep_folder = path.substr(0, slash_pos) + "/"; - - reglib::Model * fullmodel = processAV(path); - - //savePoses(sweep_folder+"testposes.xml",fullmodel->relativeposes,17); - - writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( fullmodel, reg); - mu->occlusion_penalty = 15; - mu->massreg_timeout = 60*4; - mu->viewer = viewer; - - std::vector po; - std::vector fr; - std::vector mm; - fullmodel->getData(po, fr, mm); - //fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); - //fullmodel->recomputeModelPoints(); - - SimpleXMLParser parser; - SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); - std::string current_waypointid = current_roomData.roomWaypointId; - - if(overall_folder.back() == '/'){overall_folder.pop_back();} - - int prevind = -1; - std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); - for (unsigned int i = 0; i < sweep_xmls.size(); i++){ - SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); - std::string other_waypointid = other_roomData.roomWaypointId; - - if(sweep_xmls[i].compare(path) == 0){break;} - if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} - } - - - if(prevind != -1){ - std::string prev = sweep_xmls[prevind]; - printf("prev: %s\n",prev.c_str()); - - reglib::Model * bg = quasimodo_brain::load_metaroom_model(prev); - //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); - //bg->recomputeModelPoints(); - - std::vector< reglib::Model * > models; - models.push_back(fullmodel); - - std::vector< std::vector< cv::Mat > > internal; - std::vector< std::vector< cv::Mat > > external; - std::vector< std::vector< cv::Mat > > dynamic; - - quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization_lvl > 0); - - remove_old_seg(sweep_folder); - - if(models.size() == 0){ - returnval = 2; - }else{ - returnval = 3; - } - - for(unsigned int i = 0; i < models.size(); i++){ - std::vector internal_masks = internal[i]; - std::vector external_masks = external[i]; - std::vector dynamic_masks = dynamic[i]; - reglib::Model * model = models[i]; - - std::vector mod_po; - std::vector mod_fr; - std::vector mod_mm; - model->getData(mod_po, mod_fr, mod_mm); - - int dynamicCounter = 1; - while(true){ - double sum = 0; - double sumx = 0; - double sumy = 0; - double sumz = 0; - std::vector imgnr; - std::vector masks; - - for(unsigned int j = 0; j < mod_fr.size(); j++){ - reglib::RGBDFrame * frame = mod_fr[j]; - //std::cout << mod_po[j] << std::endl; - Eigen::Matrix4d p = mod_po[j]*bg->frames.front()->pose; - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); - - reglib::Camera * camera = frame->camera; - - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); - unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); - unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - cv::Mat mask; - mask.create(height,width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - - - bool containsData = false; - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - - if(dynamicmaskdata[ind] == dynamicCounter){ - maskdata[ind] = 255; - containsData = true; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - sumx += m00*x + m01*y + m02*z + m03; - sumy += m10*x + m11*y + m12*z + m13; - sumz += m20*x + m21*y + m22*z + m23; - sum ++; - } - }else{ - maskdata[ind] = 0; - } - } - } - - if(containsData){ - masks.push_back(mask); - imgnr.push_back(j); - } - } - if(masks.size() > 0){ - char buf [1024]; - sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),dynamicCounter-1); - QFile file(buf); - if (file.exists()){file.remove();} - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("Object"); - xmlWriter->writeAttribute("object_number", QString::number(dynamicCounter-1)); - xmlWriter->writeStartElement("Mean"); - xmlWriter->writeAttribute("x", QString::number(sumx/sum)); - xmlWriter->writeAttribute("y", QString::number(sumy/sum)); - xmlWriter->writeAttribute("z", QString::number(sumz/sum)); - xmlWriter->writeEndElement(); - - - for(unsigned int j = 0; j < masks.size(); j++){ - char buf [1024]; - sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),dynamicCounter-1,imgnr[j]); - cv::imwrite(buf, masks[j] ); - - sprintf(buf,"dynamicmask_%i_%i.png",dynamicCounter-1,imgnr[j]); - xmlWriter->writeStartElement("Mask"); - xmlWriter->writeAttribute("filename", QString(buf)); - xmlWriter->writeAttribute("image_number", QString::number(imgnr[j])); - xmlWriter->writeEndElement(); - } - - xmlWriter->writeEndElement(); - xmlWriter->writeEndDocument(); - delete xmlWriter; - dynamicCounter++; - }else{break;} - } - - int movingCounter = 1; - while(true){ - double sum = 0; - double sumx = 0; - double sumy = 0; - double sumz = 0; - std::vector imgnr; - std::vector masks; - - for(unsigned int j = 0; j < mod_fr.size(); j++){ - reglib::RGBDFrame * frame = mod_fr[j]; - //std::cout << mod_po[j] << std::endl; - Eigen::Matrix4d p = mod_po[j]*bg->frames.front()->pose; - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); - - reglib::Camera * camera = frame->camera; - - unsigned char * internalmaskdata = (unsigned char *)(internal_masks[j].data); - unsigned char * externalmaskdata = (unsigned char *)(external_masks[j].data); - unsigned char * dynamicmaskdata = (unsigned char *)(dynamic_masks[j].data); - - float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); - float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); - float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); - - const float idepth = camera->idepth_scale; - const float cx = camera->cx; - const float cy = camera->cy; - const float ifx = 1.0/camera->fx; - const float ify = 1.0/camera->fy; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - cv::Mat mask; - mask.create(height,width,CV_8UC1); - unsigned char * maskdata = (unsigned char *)(mask.data); - - - bool containsData = false; - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - - if(internalmaskdata[ind] == movingCounter){ - maskdata[ind] = 255; - containsData = true; - float z = idepth*float(depthdata[ind]); - if(z > 0){ - float x = (float(w) - cx) * z * ifx; - float y = (float(h) - cy) * z * ify; - - sumx += m00*x + m01*y + m02*z + m03; - sumy += m10*x + m11*y + m12*z + m13; - sumz += m20*x + m21*y + m22*z + m23; - sum ++; - } - }else{ - maskdata[ind] = 0; - } - } - } - - if(containsData){ - masks.push_back(mask); - imgnr.push_back(j); - } - } - if(masks.size() > 0){ - char buf [1024]; - sprintf(buf,"%s/moving_object%10.10i.xml",sweep_folder.c_str(),movingCounter-1); - QFile file(buf); - if (file.exists()){file.remove();} - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("Object"); - xmlWriter->writeAttribute("object_number", QString::number(movingCounter-1)); - xmlWriter->writeStartElement("Mean"); - xmlWriter->writeAttribute("x", QString::number(sumx/sum)); - xmlWriter->writeAttribute("y", QString::number(sumy/sum)); - xmlWriter->writeAttribute("z", QString::number(sumz/sum)); - xmlWriter->writeEndElement(); - - for(unsigned int j = 0; j < masks.size(); j++){ - char buf [1024]; - sprintf(buf,"%s/movingmask_%i_%i.png",sweep_folder.c_str(),movingCounter-1,imgnr[j]); - cv::imwrite(buf, masks[j] ); - - sprintf(buf,"movingmask_%i_%i.png",movingCounter-1,imgnr[j]); - xmlWriter->writeStartElement("Mask"); - xmlWriter->writeAttribute("filename", QString(buf)); - xmlWriter->writeAttribute("image_number", QString::number(imgnr[j])); - xmlWriter->writeEndElement(); - } - - xmlWriter->writeEndElement(); - xmlWriter->writeEndDocument(); - delete xmlWriter; - movingCounter++; - }else{break;} - } - } - bg->fullDelete(); - delete bg; - }else{ - returnval = 1; - } - - fullmodel->fullDelete(); - delete fullmodel; - // delete bgmassreg; - delete reg; - delete mu; - printf("publishing file %s to %s\n",path.c_str(),outtopic.c_str()); - - std_msgs::String msg; - msg.data = path; - for(unsigned int i = 0; i < out_pubs.size(); i++){out_pubs[i].publish(msg);} - ros::spinOnce(); - - return returnval; -} - -void chatterCallback(const std_msgs::String::ConstPtr& msg){processMetaroom(msg->data);} - -void trainMetaroom(std::string path){ - printf("processing: %s\n",path.c_str()); - if(posepath.compare("")==0){ - printf("posepath not set, set before training\n"); - return ; - } - - int slash_pos = path.find_last_of("/"); - std::string sweep_folder = path.substr(0, slash_pos) + "/"; - - reglib::Model * fullmodel = processAV(path); - - //Not needed if metaroom well calibrated - reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); - bgmassreg->timeout = 600; - bgmassreg->viewer = viewer; - bgmassreg->use_surface = true; - bgmassreg->use_depthedge = true; - bgmassreg->visualizationLvl = 0; - bgmassreg->maskstep = 5; - bgmassreg->nomaskstep = 5; - bgmassreg->nomask = true; - bgmassreg->stopval = 0.0005; - bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); - reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); - - savePoses(overall_folder+"/"+posepath,bgmfr.poses,17); - fullmodel->fullDelete(); - delete fullmodel; - delete bgmassreg; -} - -std::vector loadModels(std::string path){ - printf("loadModels: %s\n",path.c_str()); - std::vector models; - int slash_pos = path.find_last_of("/"); - std::string sweep_folder = path.substr(0, slash_pos) + "/"; - - std::vector frames; - std::vector poses; - readViewXML(sweep_folder+"ViewGroup.xml",frames,poses); - - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_object*.xml")); - for (auto objectFile : objectFiles){ - std::string object = sweep_folder+objectFile.toStdString(); - printf("object: %s\n",object.c_str()); - - QFile file(object.c_str()); - - if (!file.exists()){ - ROS_ERROR("Could not open file %s to masks.",object.c_str()); - continue; - } - - file.open(QIODevice::ReadOnly); - ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()){ - QXmlStreamReader::TokenType token = xmlReader->readNext(); - if (token == QXmlStreamReader::StartDocument) - continue; - - if (xmlReader->hasError()){ - ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); - break; - } - - QString elementName = xmlReader->name().toString(); - - if (token == QXmlStreamReader::StartElement){ - if (xmlReader->name() == "Mask"){ - int number = 0; - cv::Mat mask; - QXmlStreamAttributes attributes = xmlReader->attributes(); - if (attributes.hasAttribute("filename")){ - QString maskpath = attributes.value("filename").toString(); - //printf("mask filename: %s\n",(sweep_folder+maskpath.toStdString()).c_str()); - mask = cv::imread(sweep_folder+"/"+(maskpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); - //mask = cv::imread((maskpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); - }else{break;} - - - if (attributes.hasAttribute("image_number")){ - QString depthpath = attributes.value("image_number").toString(); - number = atoi(depthpath.toStdString().c_str()); - printf("number: %i\n",number); - }else{break;} - - mod->frames.push_back(frames[number]->clone()); - mod->relativeposes.push_back(poses[number]); - mod->modelmasks.push_back(new reglib::ModelMask(mask)); - } - } - } - - delete xmlReader; - models.push_back(mod); - } - - for(unsigned int i = 0; i < frames.size(); i++){delete frames[i];} - - return models; -} - -void addModelToModelServer(reglib::Model * model){ - printf("addModelToModelServer\n"); - for(unsigned int i = 0; i < model_pubs.size(); i++){model_pubs[i].publish(quasimodo_brain::getModelMSG(model));} - ros::spinOnce(); -} - -void sendMetaroomToServer(std::string path){ - std::vector mods = loadModels(path); - for(unsigned int i = 0; i < mods.size(); i++){ - addModelToModelServer(mods[i]); - mods[i]->fullDelete(); - delete mods[i]; - } -} - - -void sendCallback(const std_msgs::String::ConstPtr& msg){sendMetaroomToServer(msg->data);} - -//string waypoint_id -//--- -//string[] object_id -//sensor_msgs/PointCloud2[] objects -//geometry_msgs/Point[] centroids - - -bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res){ - printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n"); - std::string current_waypointid = req.waypoint_id; - - - if(overall_folder.back() == '/'){overall_folder.pop_back();} - - SimpleXMLParser parser; - int prevind = -1; - std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); - for (unsigned int i = 0; i < sweep_xmls.size(); i++){ - SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); - std::string other_waypointid = other_roomData.roomWaypointId; - if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} - } - if(prevind == -1){return false;} - std::string path = sweep_xmls[prevind]; - //processMetaroom(path); - - printf("path: %s\n",path.c_str()); - int slash_pos = path.find_last_of("/"); - std::string sweep_folder = path.substr(0, slash_pos) + "/"; - - printf("sweep_folder: %s\n",sweep_folder.c_str()); - - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_object*.xml")); - for (auto objectFile : objectFiles){ - std::string object = sweep_folder+objectFile.toStdString(); - printf("object: %s\n",object.c_str()); - - QFile file(object.c_str()); - - if (!file.exists()){ - ROS_ERROR("Could not open file %s to masks.",object.c_str()); - continue; - } - - file.open(QIODevice::ReadOnly); - ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()){ - QXmlStreamReader::TokenType token = xmlReader->readNext(); - if (token == QXmlStreamReader::StartDocument) - continue; - - if (xmlReader->hasError()){ - ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); - break; - } - - QString elementName = xmlReader->name().toString(); - if (token == QXmlStreamReader::StartElement){ - if (xmlReader->name() == "Mean"){ - QXmlStreamAttributes attributes = xmlReader->attributes(); - double x = atof(attributes.value("x").toString().toStdString().c_str()); - double y = atof(attributes.value("y").toString().toStdString().c_str()); - double z = atof(attributes.value("z").toString().toStdString().c_str()); - printf("mean: %f %f %f\n",x,y,z); - - res.object_id.push_back(object); - sensor_msgs::PointCloud2 pc; - res.objects.push_back(pc); - geometry_msgs::Point p; - p.x = x; - p.y = y; - p.z = z; - res.centroids.push_back(p); - } - } - } - delete xmlReader; - } - - return true; -} - -bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res){ -// string waypoint_id -// string object_id -// --- -// sensor_msgs/PointCloud2 object_cloud -// int32[] object_mask -// geometry_msgs/Transform transform_to_map -// int32 pan_angle -// int32 tilt_angle - - std::string current_waypointid = req.waypoint_id; - - - if(overall_folder.back() == '/'){overall_folder.pop_back();} - - SimpleXMLParser parser; - int prevind = -1; - std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); - for (unsigned int i = 0; i < sweep_xmls.size(); i++){ - SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); - std::string other_waypointid = other_roomData.roomWaypointId; - if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} - } - if(prevind == -1){return false;} - std::string path = sweep_xmls[prevind]; - //processMetaroom(path); - - printf("path: %s\n",path.c_str()); - int slash_pos = path.find_last_of("/"); - std::string sweep_folder = path.substr(0, slash_pos) + "/"; - - printf("sweep_folder: %s\n",sweep_folder.c_str()); - char buf [1024]; - sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),req.object_id.c_str()); - - std::string path = std::string(buf); - -// res.object_mask = object.object_indices; -// tf::transformTFToMsg(object.transform_to_map, res.transform_to_map); -// pcl::toROSMsg(*object.object_cloud, res.object_cloud); -// res.object_cloud.header.frame_id="/map"; -// res.pan_angle = -object.pan_angle; -// res.tilt_angle = -object.tilt_angle; - - return true; -} - - - -bool testDynamicObjectServiceCallback(std::string path){ - printf("bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res)\n"); - DynamicObjectsServiceRequest req; - req.waypoint_id = path; - DynamicObjectsServiceResponse res; - return dynamicObjectsServiceCallback(req,res); -} - -void roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& obs_msg) { - std::cout<<"Room obs message received"<xml_file_name); - - std_msgs::String msg; - if(ret == 0){ - ROS_ERROR_STREAM("Xml file does not exist. Aborting."); - msg.data = "error_processing_observation"; - }else{msg.data = "finished_processing_observation";} - - if(ret == 1){ROS_ERROR_STREAM("First metaroom.");} - if(ret == 2){ROS_ERROR_STREAM("No moving objects found.");} - if(ret == 3){ROS_ERROR_STREAM("Moving objects found.");} - - for(unsigned int i = 0; i < m_PublisherStatuss.size(); i++){m_PublisherStatuss[i].publish(msg);} -} - - -void setLargeStack(){ - const rlim_t kStackSize = 256 * 1024 * 1024; // min stack size = 256 MB - struct rlimit rl; - unsigned long result; - - result = getrlimit(RLIMIT_STACK, &rl); - if (result == 0){ - if (rl.rlim_cur < kStackSize){ - rl.rlim_cur = kStackSize; - result = setrlimit(RLIMIT_STACK, &rl); - if (result != 0){fprintf(stderr, "setrlimit returned result = %d\n", int(result));} - } - } -} - -int main(int argc, char** argv){ - - bool baseSetting = true; - bool once = false; - - overall_folder = std::string(getenv ("HOME"))+"/.semanticMap/"; - outtopic = "/some/topic"; - modelouttopic = "/model/out/topic"; - posepath = "testposes.xml"; - - ros::init(argc, argv, "metaroom_additional_view_processing"); - ros::NodeHandle n; - - std::vector segsubs; - std::vector sendsubs; - std::vector roomObservationSubs; - - std::vector trainMetarooms; - std::vector sendMetaroomToServers; - std::vector processMetarooms; - - int inputstate = 0; - for(int i = 1; i < argc;i++){ - printf("input: %s\n",argv[i]); - if( std::string(argv[i]).compare("-intopic") == 0){ inputstate = 0;} - else if(std::string(argv[i]).compare("-outtopic") == 0){ inputstate = 1;} - else if(std::string(argv[i]).compare("-file") == 0){ inputstate = 2;} - else if(std::string(argv[i]).compare("-v") == 0){ - viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0.5, 0, 0.5); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); - visualization_lvl = 1; - inputstate = 3; - } - else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 4;} - else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} - else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} - else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} - else if(std::string(argv[i]).compare("-sendModel") == 0){ inputstate = 8;} - else if(std::string(argv[i]).compare("-sendSub") == 0) { inputstate = 9;} - else if(std::string(argv[i]).compare("-sendTopic") == 0){ inputstate = 10;} - else if(std::string(argv[i]).compare("-roomObservationTopic") == 0){ inputstate = 11;} - else if(std::string(argv[i]).compare("-DynamicObjectsService") == 0){ inputstate = 12;} - else if(std::string(argv[i]).compare("-GetDynamicObjectService") == 0){ inputstate = 13;} - else if(std::string(argv[i]).compare("-statusmsg") == 0){ inputstate = 14;} - else if(std::string(argv[i]).compare("-once") == 0){ once = true;} - else if(std::string(argv[i]).compare("-nobase") == 0){ baseSetting = false;} - else if(inputstate == 0){ - segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); - }else if(inputstate == 1){ - out_pubs.push_back(n.advertise(std::string(argv[i]), 1000)); - }else if(inputstate == 2){ - processMetarooms.push_back(std::string(argv[i])); - }else if(inputstate == 3){ - visualization_lvl = atoi(argv[i]); - }else if(inputstate == 4){ - overall_folder = std::string(argv[i]); - }else if(inputstate == 5){ - trainMetarooms.push_back(std::string(argv[i])); - }else if(inputstate == 6){ - posepath = std::string(argv[i]); - }else if(inputstate == 7){ - sweepPoses = readPoseXML(std::string(argv[i])); - }else if(inputstate == 8){ - sendMetaroomToServers.push_back(std::string(argv[i])); - }else if(inputstate == 9){ - sendsubs.push_back(n.subscribe(std::string(argv[i]), 1000, sendCallback)); - }else if(inputstate == 10){ - model_pubs.push_back(n.advertise(std::string(argv[i]), 1000)); - }else if(inputstate == 11){ - roomObservationSubs.push_back(n.subscribe(std::string(argv[i]), 1000, roomObservationCallback)); - }else if(inputstate == 12){ - m_DynamicObjectsServiceServers.push_back(n.advertiseService(std::string(argv[i]), dynamicObjectsServiceCallback)); - }else if(inputstate == 13){ - m_GetDynamicObjectServiceServers.push_back(n.advertiseService(std::string(argv[i]), getDynamicObjectServiceCallback)); - }else if(inputstate == 14){ - m_PublisherStatuss.push_back(n.advertise(std::string(argv[i]), 1000)); - } - } - - if(baseSetting){ - if(m_PublisherStatuss.size() == 0){ - m_PublisherStatuss.push_back(n.advertise("/local_metric_map/status", 1000)); - } - - if(segsubs.size() == 0){ - segsubs.push_back(n.subscribe("/some/inputtopic", 1000, chatterCallback)); - } - - if(out_pubs.size() == 0){ - out_pubs.push_back(n.advertise("/some/topic", 1000)); - } - - if(model_pubs.size() == 0){ - model_pubs.push_back(n.advertise("/model/out/topic", 1000)); - } - - if(sendsubs.size() == 0){ - sendsubs.push_back(n.subscribe("/model/out/topic/path", 1000, sendCallback)); - } - - if(roomObservationSubs.size() == 0){ - roomObservationSubs.push_back(n.subscribe("/local_metric_map/room_observations", 1000, roomObservationCallback)); - } - - if(m_DynamicObjectsServiceServers.size() == 0){ - m_DynamicObjectsServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback)); - } - - if(m_GetDynamicObjectServiceServers.size() == 0){ - m_GetDynamicObjectServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/GetDynamicObjectService", getDynamicObjectServiceCallback)); - } - } - - printf("overall_folder: %s\n",overall_folder.c_str()); - - for(unsigned int i = 0; i < trainMetarooms.size(); i++){ trainMetaroom( trainMetarooms[i]);} - for(unsigned int i = 0; i < processMetarooms.size(); i++){ processMetaroom( processMetarooms[i]);} - for(unsigned int i = 0; i < sendMetaroomToServers.size(); i++){ sendMetaroomToServer( sendMetaroomToServers[i]);} - - if(!once){ros::spin();} - printf("done...\n"); - return 0; -} From bb80efefeb6ab12e24e700cc693cdfb5ffc3dd2a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 23:01:46 +0200 Subject: [PATCH 146/255] ... --- .../src/learn_objects_action/metric_sweep.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index 65788714..630eb1a0 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -180,16 +180,23 @@ def execute(self, userdata): print "LINE:4" rospy.logwarn( "Getting cluster: %s"%clusters.object_id[ID]) #one_cluster = self._get_specific_cluster(userdata['waypoint'], clusters.object_id[ID]) - + print "LINE:5" #one_cluster = self._get_specific_cluster(userdata.action_goal.waypoint, # clusters.object_id[ID]) #userdata['dynamic_object']=one_cluster + print "LINE:6" userdata.dynamic_object_centroid = clusters.centroids[ID] + print "LINE:6.5" userdata.dynamic_object_points = clusters.objects[ID] + print "LINE:6.75" userdata.dynamic_object_id = clusters.object_id[ID] + print "LINE:7" world = World() + print "LINE:8" userdata['object'] = world.create_object() + print "LINE:9" userdata['object'].add_identification('ObjectLearnerID', ObjectIdentification({'NEW':1})) + print "LINE:10" print clusters.object_id[ID] print "="*20 print clusters.centroids[ID] From a8992681d6ffb62f92d64804deb41830c4da1814 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 4 Oct 2016 23:11:20 +0200 Subject: [PATCH 147/255] ... --- .../src/learn_objects_action/metric_sweep.py | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index 630eb1a0..65788714 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -180,23 +180,16 @@ def execute(self, userdata): print "LINE:4" rospy.logwarn( "Getting cluster: %s"%clusters.object_id[ID]) #one_cluster = self._get_specific_cluster(userdata['waypoint'], clusters.object_id[ID]) - print "LINE:5" + #one_cluster = self._get_specific_cluster(userdata.action_goal.waypoint, # clusters.object_id[ID]) #userdata['dynamic_object']=one_cluster - print "LINE:6" userdata.dynamic_object_centroid = clusters.centroids[ID] - print "LINE:6.5" userdata.dynamic_object_points = clusters.objects[ID] - print "LINE:6.75" userdata.dynamic_object_id = clusters.object_id[ID] - print "LINE:7" world = World() - print "LINE:8" userdata['object'] = world.create_object() - print "LINE:9" userdata['object'].add_identification('ObjectLearnerID', ObjectIdentification({'NEW':1})) - print "LINE:10" print clusters.object_id[ID] print "="*20 print clusters.centroids[ID] From 2ec8e2420cb734d0657df31be902bc6ed9574c13 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 6 Oct 2016 22:50:36 +0200 Subject: [PATCH 148/255] ... --- quasimodo/quasimodo_brain/CMakeLists.txt | 2 +- quasimodo/quasimodo_brain/package.xml | 2 + quasimodo/quasimodo_brain/src/Util/Util.cpp | 3 +- .../metaroom_additional_view_processing.cpp | 247 +++++++++++++++--- .../launch/semantic_map.launch | 4 +- 5 files changed, 212 insertions(+), 46 deletions(-) diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index a31771bf..e6800e1b 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(quasimodo_brain) -find_package(catkin REQUIRED COMPONENTS roscpp soma_msgs object_manager_msgs semantic_map_msgs quasimodo_msgs quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge) +find_package(catkin REQUIRED COMPONENTS roscpp soma_msgs object_manager object_manager_msgs semantic_map_msgs quasimodo_msgs quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge) set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fPIC -std=c++0x -o popcnt -mssse3") diff --git a/quasimodo/quasimodo_brain/package.xml b/quasimodo/quasimodo_brain/package.xml index 15c938bd..f11063cd 100644 --- a/quasimodo/quasimodo_brain/package.xml +++ b/quasimodo/quasimodo_brain/package.xml @@ -58,6 +58,8 @@ cv_bridge semantic_map_msgs object_manager_msgs + object_manager + object_manager object_manager_msgs semantic_map_msgs quasimodo_models diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index c7671895..d12c53eb 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -282,6 +282,7 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} masks.push_back(mask); } + std::vector movemask; std::vector dynmask; printf("computeMovingDynamicStatic\n"); @@ -289,8 +290,6 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec external.push_back(movemask); internal.push_back(masks); dynamic.push_back(dynmask); - - //SegmentationResults sr = mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions } delete reg; diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 89e5b68c..f3896b6a 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -78,6 +78,11 @@ #include // Custom messages +#include "object_manager/dynamic_object.h" +#include "object_manager/dynamic_object_xml_parser.h" +#include "object_manager/dynamic_object_utilities.h" +#include "object_manager/dynamic_object_mongodb_interface.h" + #include #include @@ -136,8 +141,9 @@ void remove_old_seg(std::string sweep_folder){ /* print all the files and directories within directory */ while ((ent = readdir (dir)) != NULL) { std::string file = std::string(ent->d_name); + printf("file: %s\n",file.c_str()); - if (file.find("dynamic_object") !=std::string::npos && file.find(".xml") !=std::string::npos){ + if (file.find("dynamic_obj") !=std::string::npos && (file.find(".xml") !=std::string::npos || file.find(".pcd") !=std::string::npos)){ printf ("removing %s\n", ent->d_name); std::remove((sweep_folder+"/"+file).c_str()); } @@ -147,7 +153,7 @@ void remove_old_seg(std::string sweep_folder){ std::remove((sweep_folder+"/"+file).c_str()); } - if (file.find("moving_object") !=std::string::npos && file.find(".xml") !=std::string::npos){ + if (file.find("moving_obj") !=std::string::npos && (file.find(".xml") !=std::string::npos || file.find(".pcd") !=std::string::npos)){ printf ("removing %s\n", ent->d_name); std::remove((sweep_folder+"/"+file).c_str()); } @@ -156,6 +162,17 @@ void remove_old_seg(std::string sweep_folder){ printf ("removing %s\n", ent->d_name); std::remove((sweep_folder+"/"+file).c_str()); } + +// if (file.find("_object_") !=std::string::npos && file.find(".xml") !=std::string::npos){ +// printf ("removing %s\n", ent->d_name); +// std::remove((sweep_folder+"/"+file).c_str()); +// } +// if (file.find("_object_") !=std::string::npos && file.find(".pcd") !=std::string::npos){ +// printf ("maybe: removing %s\n", ent->d_name); +// printf("%i\n",file.find("_additional_view_") == std::string::npos); +// //file.find("_additional_view_") == std::string::npos; +// //std::remove((sweep_folder+"/"+file).c_str()); +// } } closedir (dir); } @@ -578,8 +595,6 @@ reglib::Model * processAV(std::string path){ delete mu; delete sweep; - - return fullmodel; } @@ -653,11 +668,12 @@ std::vector readPoseXML(std::string xmlFile){ return poses; } -int processMetaroom(std::string path){ +int processMetaroom(std::string path, bool store_old_xml = true){ int returnval = 0; printf("processing: %s\n",path.c_str()); - // testDynamicObjectServiceCallback(path); - // exit(0); + +// testDynamicObjectServiceCallback(path); +// exit(0); if ( ! boost::filesystem::exists( path ) ){return 0;} @@ -685,6 +701,14 @@ int processMetaroom(std::string path){ SimpleXMLParser parser; SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); + +// SemanticRoom observation = SemanticRoomXMLParser::loadRoomFromXML(path,false); +// Eigen::Matrix4f roomTransform = observation.getRoomTransform(); +// printf("roomTransform\n"); +// std::cout << roomTransform << std::endl; + + DynamicObjectXMLParser objectparser(path, true); + std::string current_waypointid = current_roomData.roomWaypointId; if(overall_folder.back() == '/'){overall_folder.pop_back();} @@ -705,6 +729,7 @@ int processMetaroom(std::string path){ printf("prev: %s\n",prev.c_str()); reglib::Model * bg = quasimodo_brain::load_metaroom_model(prev); + auto sweep = SimpleXMLParser::loadRoomFromXML(prev, std::vector{},false); //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); //bg->recomputeModelPoints(); @@ -745,10 +770,14 @@ int processMetaroom(std::string path){ std::vector imgnr; std::vector masks; + CloudPtr cloud_cluster (new Cloud()); + + Eigen::Matrix4d first = model->frames.front()->pose; + for(unsigned int j = 0; j < mod_fr.size(); j++){ reglib::RGBDFrame * frame = mod_fr[j]; - //std::cout << mod_po[j] << std::endl; - Eigen::Matrix4d p = mod_po[j]*bg->frames.front()->pose; + std::cout << first*mod_po[j] << std::endl; + Eigen::Matrix4d p = frame->pose;//first*mod_po[j];//*bg->frames.front()->pose; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); unsigned short * depthdata = (unsigned short *)(frame->depth.data); float * normalsdata = (float *)(frame->normals.data); @@ -789,9 +818,18 @@ int processMetaroom(std::string path){ float x = (float(w) - cx) * z * ifx; float y = (float(h) - cy) * z * ify; - sumx += m00*x + m01*y + m02*z + m03; - sumy += m10*x + m11*y + m12*z + m13; - sumz += m20*x + m21*y + m22*z + m23; + PointType p; + p.x += m00*x + m01*y + m02*z + m03; + p.y += m10*x + m11*y + m12*z + m13; + p.z += m20*x + m21*y + m22*z + m23; + p.r = rgbdata[3*ind+2]; + p.g = rgbdata[3*ind+1]; + p.b = rgbdata[3*ind+0]; + cloud_cluster->points.push_back (p); + + sumx += p.x; + sumy += p.y; + sumz += p.z; sum ++; } }else{ @@ -805,9 +843,43 @@ int processMetaroom(std::string path){ imgnr.push_back(j); } } + + cloud_cluster->width = cloud_cluster->points.size (); + cloud_cluster->height = 1; + cloud_cluster->is_dense = true; + if(masks.size() > 0){ + + if(store_old_xml){ + std::stringstream ss; + ss << "object_"; + ss << (dynamicCounter-1); + + // create and save dynamic object + DynamicObject::Ptr roomObject(new DynamicObject()); + roomObject->setCloud(cloud_cluster); + roomObject->setTime(sweep.roomLogStartTime); + roomObject->m_roomLogString = sweep.roomLogName; + roomObject->m_roomStringId = sweep.roomWaypointId; + roomObject->m_roomRunNumber = sweep.roomRunNumber; + // // create label from room log time; could be useful later on, and would resolve ambiguities + std::stringstream ss_obj; + ss_obj<m_label = ss_obj.str(); + std::string xml_file = objectparser.saveAsXML(roomObject); + printf("xml_file: %s\n",xml_file.c_str()); + } + char buf [1024]; - sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),dynamicCounter-1); + sprintf(buf,"%s/dynamic_obj%10.10i.pcd",sweep_folder.c_str(),dynamicCounter-1); + pcl::io::savePCDFileBinaryCompressed(std::string(buf),*cloud_cluster); + + +// std::string objectpcd = std::string(buf);//object.substr(0,object.size()-4); +// std::cout << objectpcd.substr(0,objectpcd.size()-4) << std::endl; + + sprintf(buf,"%s/dynamic_obj%10.10i.xml",sweep_folder.c_str(),dynamicCounter-1); QFile file(buf); if (file.exists()){file.remove();} if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"< 0){ char buf [1024]; - sprintf(buf,"%s/moving_object%10.10i.xml",sweep_folder.c_str(),movingCounter-1); + sprintf(buf,"%s/moving_obj%10.10i.xml",sweep_folder.c_str(),movingCounter-1); QFile file(buf); if (file.exists()){file.remove();} if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"< loadModels(std::string path){ std::vector poses; readViewXML(sweep_folder+"ViewGroup.xml",frames,poses); - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_object*.xml")); + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml")); for (auto objectFile : objectFiles){ std::string object = sweep_folder+objectFile.toStdString(); printf("object: %s\n",object.c_str()); @@ -1108,6 +1179,8 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n"); std::string current_waypointid = req.waypoint_id; + printf("current_waypointid: %i\n",current_waypointid.c_str()); + if(overall_folder.back() == '/'){overall_folder.pop_back();} @@ -1165,13 +1238,25 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj double z = atof(attributes.value("z").toString().toStdString().c_str()); printf("mean: %f %f %f\n",x,y,z); - res.object_id.push_back(object); - sensor_msgs::PointCloud2 pc; - res.objects.push_back(pc); + std::string objectpcd = object.substr(0,object.size()-4); + std::cout << objectpcd << std::endl; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::io::loadPCDFile (objectpcd+".pcd", *cloud); + + printf("cloud->points.size() = %i\n",cloud->points.size()); + + sensor_msgs::PointCloud2 msg_objects; + pcl::toROSMsg(*cloud, msg_objects); + msg_objects.header.frame_id="/map"; + geometry_msgs::Point p; p.x = x; p.y = y; p.z = z; + + res.object_id.push_back(object); + res.objects.push_back(msg_objects); res.centroids.push_back(p); } } @@ -1183,14 +1268,14 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj } bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res){ -// string waypoint_id -// string object_id -// --- -// sensor_msgs/PointCloud2 object_cloud -// int32[] object_mask -// geometry_msgs/Transform transform_to_map -// int32 pan_angle -// int32 tilt_angle + // string waypoint_id + // string object_id + // --- + // sensor_msgs/PointCloud2 object_cloud + // int32[] object_mask + // geometry_msgs/Transform transform_to_map + // int32 pan_angle + // int32 tilt_angle std::string current_waypointid = req.waypoint_id; @@ -1217,20 +1302,100 @@ bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDyn printf("req.object_id: %s\n",req.object_id.c_str()); char buf [1024]; //sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),atoi(req.object_id.c_str()); -//sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),atoi(req.object_id.c_str()); + //sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),atoi(req.object_id.c_str()); std::string fullpath = std::string(buf); -// res.object_mask = object.object_indices; -// tf::transformTFToMsg(object.transform_to_map, res.transform_to_map); -// pcl::toROSMsg(*object.object_cloud, res.object_cloud); -// res.object_cloud.header.frame_id="/map"; -// res.pan_angle = -object.pan_angle; -// res.tilt_angle = -object.tilt_angle; - +// best_transform = roomTransform.inverse() * best_transform; +// const Eigen::Affine3d eigenTr(best_transform.cast()); +// tf::transformEigenToTF(eigenTr,returned_object.transform_to_map); +// returned_object.object_cloud = CloudPtr(new Cloud()); +// *returned_object.object_cloud = *allClouds[best_index]; +// // pcl::transformPointCloud (*returned_object.object_cloud, *returned_object.object_cloud, best_transform); // transform to map frame + +// returned_object.object_indices.insert(returned_object.object_indices.begin(), src_indices.begin(),src_indices.end()); +// returned_object.object_mask = cluster_image; + +// // save mask and indices +// if (m_bSaveMask) { +// // find observation folder +// int slash_pos = observation_xml.find_last_of("/"); +// std::string observation_folder = observation_xml.substr(0, slash_pos); +// std::string mask_image = observation_folder + "/" + object_id + "_mask.jpg"; +// cv::imwrite(mask_image, cluster_image); +// std::string mask_indices = observation_folder + "/" + object_id + "_mask.txt"; +// ofstream mask_indices_os; +// mask_indices_os.open(mask_indices); +// for (int index : returned_object.object_indices){ +// mask_indices_os << index<<" "; +// } +// mask_indices_os.close(); +// ROS_INFO_STREAM("Object mask saved at: "< currentObjects; + bool objects_found = updateObjectsAtWaypoint(req.waypoint_id); + if (!objects_found){return true;} + + GetObjStruct object; + bool found =returnObjectMask(req.waypoint_id, req.object_id,m_waypointToSweepFileMap[req.waypoint_id], object); + if (!found){ + ROS_ERROR_STREAM("Could not compute mask for object id "< if not, add the intermediate cloud & its transform + // if (!m_objectTracked->m_vAdditionalViews.size()){ + // m_objectTracked->addAdditionalView(object.object_cloud); + // m_objectTracked->addAdditionalViewTransform(object.transform_to_map); + // } + } + + return true; +*/ bool testDynamicObjectServiceCallback(std::string path){ printf("bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res)\n"); @@ -1379,13 +1544,13 @@ int main(int argc, char** argv){ roomObservationSubs.push_back(n.subscribe("/local_metric_map/room_observations", 1000, roomObservationCallback)); } - if(m_DynamicObjectsServiceServers.size() == 0){ - m_DynamicObjectsServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback)); - } +// if(m_DynamicObjectsServiceServers.size() == 0){ +// m_DynamicObjectsServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback)); +// } - if(m_GetDynamicObjectServiceServers.size() == 0){ - m_GetDynamicObjectServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/GetDynamicObjectService", getDynamicObjectServiceCallback)); - } +// if(m_GetDynamicObjectServiceServers.size() == 0){ +// m_GetDynamicObjectServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/GetDynamicObjectService", getDynamicObjectServiceCallback)); +// } } printf("overall_folder: %s\n",overall_folder.c_str()); diff --git a/semantic_map_launcher/launch/semantic_map.launch b/semantic_map_launcher/launch/semantic_map.launch index cab860c8..2104bec0 100644 --- a/semantic_map_launcher/launch/semantic_map.launch +++ b/semantic_map_launcher/launch/semantic_map.launch @@ -42,13 +42,13 @@ - + From 24b9d6e9932f742973ecc692b255b19c0f07233a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 7 Oct 2016 09:20:52 +0200 Subject: [PATCH 149/255] ... --- object_manager/include/object_manager/object_manager.h | 2 ++ object_manager/src/dynamic_object_utilities.cpp | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/object_manager/include/object_manager/object_manager.h b/object_manager/include/object_manager/object_manager.h index 5642c6ec..b524f080 100644 --- a/object_manager/include/object_manager/object_manager.h +++ b/object_manager/include/object_manager/object_manager.h @@ -746,6 +746,7 @@ bool ObjectManager::getDynamicObjectServiceCallback(GetDynamicObjectS template bool ObjectManager::updateObjectsAtWaypoint(std::string waypoint_id) { + printf("START bool ObjectManager::updateObjectsAtWaypoint(std::string waypoint_id)\n"); using namespace std; std::vector matchingObservations = semantic_map_load_utilties::getSweepXmlsForTopologicalWaypoint(m_dataFolder, waypoint_id); if (matchingObservations.size() == 0) @@ -816,6 +817,7 @@ bool ObjectManager::updateObjectsAtWaypoint(std::string waypoint_id) template std::vector ObjectManager::loadDynamicObjectsFromObservation(std::string obs_file) { + printf("START std::vector ObjectManager::loadDynamicObjectsFromObservation(std::string obs_file)\n"); std::vector dynamicObjects; // check if the objects have been computed already diff --git a/object_manager/src/dynamic_object_utilities.cpp b/object_manager/src/dynamic_object_utilities.cpp index b7e59132..b71fdd0f 100644 --- a/object_manager/src/dynamic_object_utilities.cpp +++ b/object_manager/src/dynamic_object_utilities.cpp @@ -5,7 +5,7 @@ using namespace std; std::vector dynamic_object_utilities::loadDynamicObjects(std::string folder, bool verbose) { - printf("loadDynamicObjects\n"); + printf("std::vector dynamic_object_utilities::loadDynamicObjects(std::string folder, bool verbose)\n"); std::vector objects; folder+=std::string("/"); @@ -24,6 +24,7 @@ std::vector dynamic_object_utilities::loadDynamicObjects(std for (size_t i=0; i Date: Fri, 7 Oct 2016 09:35:42 +0200 Subject: [PATCH 150/255] ... --- semantic_map_launcher/launch/semantic_map.launch | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/semantic_map_launcher/launch/semantic_map.launch b/semantic_map_launcher/launch/semantic_map.launch index 2104bec0..93f04261 100644 --- a/semantic_map_launcher/launch/semantic_map.launch +++ b/semantic_map_launcher/launch/semantic_map.launch @@ -42,12 +42,14 @@ - + + From 901d8be52285c2c14de651fae1d7cdb936898f76 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 7 Oct 2016 09:49:25 +0200 Subject: [PATCH 151/255] ... --- .../quasimodo_brain/src/metaroom_additional_view_processing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index f3896b6a..8c3bc2c3 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -776,7 +776,7 @@ int processMetaroom(std::string path, bool store_old_xml = true){ for(unsigned int j = 0; j < mod_fr.size(); j++){ reglib::RGBDFrame * frame = mod_fr[j]; - std::cout << first*mod_po[j] << std::endl; + //std::cout << first*mod_po[j] << std::endl; Eigen::Matrix4d p = frame->pose;//first*mod_po[j];//*bg->frames.front()->pose; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); unsigned short * depthdata = (unsigned short *)(frame->depth.data); From 3323562fe8c3a5828c6f873444e73f1f0006f93e Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 7 Oct 2016 10:09:05 +0200 Subject: [PATCH 152/255] ... --- .../src/metaroom_additional_view_processing.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 8c3bc2c3..db141ef8 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -141,7 +141,6 @@ void remove_old_seg(std::string sweep_folder){ /* print all the files and directories within directory */ while ((ent = readdir (dir)) != NULL) { std::string file = std::string(ent->d_name); - printf("file: %s\n",file.c_str()); if (file.find("dynamic_obj") !=std::string::npos && (file.find(".xml") !=std::string::npos || file.find(".pcd") !=std::string::npos)){ printf ("removing %s\n", ent->d_name); @@ -707,7 +706,7 @@ int processMetaroom(std::string path, bool store_old_xml = true){ // printf("roomTransform\n"); // std::cout << roomTransform << std::endl; - DynamicObjectXMLParser objectparser(path, true); + DynamicObjectXMLParser objectparser(sweep_folder, true); std::string current_waypointid = current_roomData.roomWaypointId; From 38ab4683e5d561f6b89107132c3b6816f6b08ca4 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 7 Oct 2016 11:08:14 +0200 Subject: [PATCH 153/255] ... --- .../src/calibrate_sweep_action_server.cpp | 12 ++++++++++-- .../src/learn_objects_action/metric_sweep.py | 3 +++ 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/calibrate_sweeps/src/calibrate_sweep_action_server.cpp b/calibrate_sweeps/src/calibrate_sweep_action_server.cpp index 19968523..6fd9a628 100644 --- a/calibrate_sweeps/src/calibrate_sweep_action_server.cpp +++ b/calibrate_sweeps/src/calibrate_sweep_action_server.cpp @@ -107,8 +107,16 @@ void execute(const calibrate_sweeps::CalibrateSweepsGoalConstPtr& goal, Server* unsigned int gy = 3; unsigned int todoy = 3; RobotContainer * rc = new RobotContainer(gx,todox,gy,todoy); - + + +// 534.191590 0.000000 315.622746 +// 0.000000 534.016892 238.568515 +//0.000000 0.000000 1.000000 +printf("%f %f %f %f\n",534.191590, 534.016892,315.622746, 238.568515); + rc->initializeCamera(534.191590, 534.016892,315.622746, 238.568515, 640, 480); +//exit(0); // initialize camera parameters from the sweep + /* if (matchingObservations.size()){ SemanticRoom aRoom = SemanticRoomXMLParser::loadRoomFromXML(matchingObservations[0],true); if (aRoom.getIntermediateCloudCameraParameters().size()){ @@ -118,7 +126,7 @@ void execute(const calibrate_sweeps::CalibrateSweepsGoalConstPtr& goal, Server* // no camera parameters saved with the sweep -> initialize optimizer with default parameters rc->initializeCamera(540.0, 540.0,319.5, 219.5, 640, 480); } - } + }*/ for (size_t i=0; imax_num_sweeps && i Date: Fri, 7 Oct 2016 11:46:43 +0200 Subject: [PATCH 154/255] ... --- .../src/learn_objects_action/metric_sweep.py | 8 ++++++-- object_manager/include/object_manager/object_manager.h | 1 + 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index ffdffd42..4ffacc26 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -189,14 +189,18 @@ def execute(self, userdata): userdata.dynamic_object_id = clusters.object_id[ID] print "LINE:5" world = World() + print "LINE:6" userdata['object'] = world.create_object() + print "LINE:7" userdata['object'].add_identification('ObjectLearnerID', ObjectIdentification({'NEW':1})) - print "LINE:6" + print "LINE:8" print clusters.object_id[ID] + print "LINE:9" print "="*20 print clusters.centroids[ID] + print "LINE:10" print "="*20 - print "LINE:7" + print "LINE:11" return "selected" except Exception, e: rospy.logwarn("Failed to select a cluster!!!!") diff --git a/object_manager/include/object_manager/object_manager.h b/object_manager/include/object_manager/object_manager.h index b524f080..df1c3afe 100644 --- a/object_manager/include/object_manager/object_manager.h +++ b/object_manager/include/object_manager/object_manager.h @@ -703,6 +703,7 @@ bool ObjectManager::getDynamicObjectServiceCallback(GetDynamicObjectS return true; } + printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); GetObjStruct object; bool found =returnObjectMask(req.waypoint_id, req.object_id,m_waypointToSweepFileMap[req.waypoint_id], object); if (!found) From b0e523b880f8b474e1b997b6e8acdf600816e14e Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 7 Oct 2016 11:55:12 +0200 Subject: [PATCH 155/255] ... --- .../include/object_manager/object_manager.h | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/object_manager/include/object_manager/object_manager.h b/object_manager/include/object_manager/object_manager.h index df1c3afe..1a486e5e 100644 --- a/object_manager/include/object_manager/object_manager.h +++ b/object_manager/include/object_manager/object_manager.h @@ -522,14 +522,16 @@ bool ObjectManager::dynamicObjectsServiceCallback(DynamicObjectsServi template bool ObjectManager::getDynamicObject(std::string waypoint, std::string object_id, DynamicObject::Ptr& object, std::string& object_observation) { + printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); auto it = m_waypointToObjMap.find(waypoint); if (it == m_waypointToObjMap.end() ) { ROS_ERROR_STREAM("No objects loaded for waypoint "+waypoint); return false; } - + printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); std::vector objects = m_waypointToObjMap[waypoint]; + printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); bool found = false; for (auto objectStruct : objects) { @@ -541,10 +543,12 @@ bool ObjectManager::getDynamicObject(std::string waypoint, std::strin break; } } + printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); if(!found) { ROS_ERROR_STREAM("Object "<::getDynamicObjectServiceCallback(GetDynamicObjectS ROS_ERROR_STREAM("Could not compute mask for object id "< Date: Fri, 7 Oct 2016 13:26:43 +0200 Subject: [PATCH 156/255] ... --- .../src/learn_objects_action/metric_sweep.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index 4ffacc26..668b98dd 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -188,11 +188,11 @@ def execute(self, userdata): userdata.dynamic_object_points = clusters.objects[ID] userdata.dynamic_object_id = clusters.object_id[ID] print "LINE:5" - world = World() + #world = World() print "LINE:6" - userdata['object'] = world.create_object() + #userdata['object'] = world.create_object() print "LINE:7" - userdata['object'].add_identification('ObjectLearnerID', ObjectIdentification({'NEW':1})) + #userdata['object'].add_identification('ObjectLearnerID', ObjectIdentification({'NEW':1})) print "LINE:8" print clusters.object_id[ID] print "LINE:9" From 3f91201877f69c5429695787a5fb7a36265be49d Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 7 Oct 2016 13:54:01 +0200 Subject: [PATCH 157/255] ... --- .../src/learn_objects_action/ptu_track.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/learn_objects_action/src/learn_objects_action/ptu_track.py b/learn_objects_action/src/learn_objects_action/ptu_track.py index 147aa901..21e1c9ad 100644 --- a/learn_objects_action/src/learn_objects_action/ptu_track.py +++ b/learn_objects_action/src/learn_objects_action/ptu_track.py @@ -33,28 +33,36 @@ def __init__(self, debug_mode, debug_services): def execute(self, userdata): + print "start TurnPTUToObject::execute" try: + print "LINE 1" if self._debug_mode: self._debug_services.set_status("Turn PTU to cluster?") proceed = self._debug_services.get_proceed(("OK",)) if proceed == "PREEMPT": return 'preempted' # start transformation + print "LINE 2" self._jnts.position=[math.radians(userdata.dynamic_object.pan_angle), math.radians(userdata.dynamic_object.tilt_angle)] + print "LINE 3" goal = PtuGotoGoal() + print "LINE 4" goal.pan = userdata.dynamic_object.pan_angle goal.tilt = userdata.dynamic_object.tilt_angle goal.pan_vel = 30 goal.tilt_vel = 30 + print "LINE 5" rospy.loginfo("SET PTU: pan: %f tilt: %f", goal.pan, goal.tilt) self.ptu_client.send_goal(goal) self.ptu_client.wait_for_result() rospy.loginfo("Reached ptu goal") + print "LINE 6" print "Capturing a new shot of that object before tracking." # So uggly force get through publisher queue for i in range(30): + print "LINE " i userdata.dynamic_object.object_cloud= rospy.wait_for_message("/head_xtion/depth_registered/points", PointCloud2) # is already there print "ok." From 9ab49d5db936237dca05ddcc4354fed68f947c5a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 7 Oct 2016 13:55:20 +0200 Subject: [PATCH 158/255] ... --- learn_objects_action/src/learn_objects_action/ptu_track.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/learn_objects_action/src/learn_objects_action/ptu_track.py b/learn_objects_action/src/learn_objects_action/ptu_track.py index 21e1c9ad..915e845d 100644 --- a/learn_objects_action/src/learn_objects_action/ptu_track.py +++ b/learn_objects_action/src/learn_objects_action/ptu_track.py @@ -62,7 +62,8 @@ def execute(self, userdata): print "Capturing a new shot of that object before tracking." # So uggly force get through publisher queue for i in range(30): - print "LINE " i + print "LINE " + print i userdata.dynamic_object.object_cloud= rospy.wait_for_message("/head_xtion/depth_registered/points", PointCloud2) # is already there print "ok." From 21d5610e1e11ead69a6a6b91eeb9509f5fc6b9ff Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 7 Oct 2016 15:12:44 +0200 Subject: [PATCH 159/255] ... --- learn_objects_action/launch/learn_objects_dependencies.launch | 3 +++ 1 file changed, 3 insertions(+) diff --git a/learn_objects_action/launch/learn_objects_dependencies.launch b/learn_objects_action/launch/learn_objects_dependencies.launch index 7bae7b07..6e052244 100644 --- a/learn_objects_action/launch/learn_objects_dependencies.launch +++ b/learn_objects_action/launch/learn_objects_dependencies.launch @@ -5,7 +5,10 @@ + + From 91ae9626e8288ff8a6af1eacac6a2019339813ba Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 7 Oct 2016 15:26:00 +0200 Subject: [PATCH 160/255] ... --- .../src/learn_objects_action/metric_sweep.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index 668b98dd..01f6a0c3 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -200,6 +200,16 @@ def execute(self, userdata): print clusters.centroids[ID] print "LINE:10" print "="*20 + + print userdata.dynamic_object_centroid + print "LINE:10" + print "="*20 + + + print userdata + print "LINE:10" + print "="*20 + print "LINE:11" return "selected" except Exception, e: From 7b81ea110c4d423522198b5eb669ceee613f1052 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 7 Oct 2016 15:50:36 +0200 Subject: [PATCH 161/255] ... --- .../src/learn_objects_action/metric_sweep.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/learn_objects_action/src/learn_objects_action/metric_sweep.py b/learn_objects_action/src/learn_objects_action/metric_sweep.py index 01f6a0c3..2b33cbde 100644 --- a/learn_objects_action/src/learn_objects_action/metric_sweep.py +++ b/learn_objects_action/src/learn_objects_action/metric_sweep.py @@ -181,9 +181,9 @@ def execute(self, userdata): rospy.logwarn( "Getting cluster: %s"%clusters.object_id[ID]) #one_cluster = self._get_specific_cluster(userdata['waypoint'], clusters.object_id[ID]) - #one_cluster = self._get_specific_cluster(userdata.action_goal.waypoint, - # clusters.object_id[ID]) - #userdata['dynamic_object']=one_cluster + one_cluster = self._get_specific_cluster(userdata.action_goal.waypoint, + clusters.object_id[ID]) + userdata['dynamic_object']=one_cluster userdata.dynamic_object_centroid = clusters.centroids[ID] userdata.dynamic_object_points = clusters.objects[ID] userdata.dynamic_object_id = clusters.object_id[ID] @@ -201,14 +201,14 @@ def execute(self, userdata): print "LINE:10" print "="*20 - print userdata.dynamic_object_centroid - print "LINE:10" - print "="*20 + #print userdata.dynamic_object_centroid + #print "LINE:10" + #print "="*20 - print userdata - print "LINE:10" - print "="*20 + #print userdata + #print "LINE:10" + #print "="*20 print "LINE:11" return "selected" From 7f5309c10ca3bed9ac1aea20519305642ce958e7 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 7 Oct 2016 16:52:25 +0200 Subject: [PATCH 162/255] ... --- learn_objects_action/launch/learn_objects_dependencies.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/learn_objects_action/launch/learn_objects_dependencies.launch b/learn_objects_action/launch/learn_objects_dependencies.launch index 6e052244..edacc765 100644 --- a/learn_objects_action/launch/learn_objects_dependencies.launch +++ b/learn_objects_action/launch/learn_objects_dependencies.launch @@ -6,9 +6,10 @@ - + From d5819a871d2d5ff4ba6468d7e249c9284733d7b0 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 10 Oct 2016 14:06:11 +0200 Subject: [PATCH 163/255] ... --- .../src/calibrate_sweep_action_server.cpp | 259 +++++++++++++++++- 1 file changed, 246 insertions(+), 13 deletions(-) diff --git a/calibrate_sweeps/src/calibrate_sweep_action_server.cpp b/calibrate_sweeps/src/calibrate_sweep_action_server.cpp index 6fd9a628..76f58dfa 100644 --- a/calibrate_sweeps/src/calibrate_sweep_action_server.cpp +++ b/calibrate_sweeps/src/calibrate_sweep_action_server.cpp @@ -16,6 +16,9 @@ #include #include +#include "ros/ros.h" +#include "std_msgs/String.h" + typedef pcl::PointXYZRGB PointType; typedef pcl::PointCloud Cloud; typedef typename Cloud::Ptr CloudPtr; @@ -23,11 +26,234 @@ using namespace std; typedef actionlib::SimpleActionServer Server; +std::string runCalibration(int min_num_sweeps = 1, int max_num_sweeps = 100000, std::string sweep_location = "", std::string save_location = ""){ + + if (sweep_location=="") + { + passwd* pw = getpwuid(getuid()); + std::string path(pw->pw_dir); + + path+="/.semanticMap/"; + sweep_location = path; + } else { + sweep_location+="/"; + } + if ( ! boost::filesystem::exists( sweep_location ) ) + { + ROS_ERROR_STREAM("Could not find folder where to load sweeps from "+sweep_location); + //as->setAborted(res,"Could not find folder where to load sweeps from "+sweep_location); + return ""; + } + + + SweepParameters complete_sweep_parameters (-160, 20, 160, -30, 30, 30); + ROS_INFO_STREAM("Calibrating using sweeps with paramters "<pw_dir); + + path+="/.ros/semanticMap/"; + save_folder = path; + if ( ! boost::filesystem::exists( save_folder ) ) + { + if (!boost::filesystem::create_directory(save_folder)) + { + ROS_ERROR_STREAM("Could not create folder where to save calibration data "+save_folder); + //as->setAborted(res,"Could not create folder where to save calibration data "+save_folder); + return ""; + } + } + ROS_INFO_STREAM("Calibration data will be saved at: "< matchingObservations = semantic_map_load_utilties::getSweepXmls(sweep_location); + + if (matchingObservations.size() < min_num_sweeps) + { + ROS_ERROR_STREAM("Not enough sweeps to perform calibration "<setAborted(res,"Not enough sweeps to perform calibration "+matchingObservations.size()); + return ""; + } + + std::string saveLocation = save_location; + if (saveLocation == "") + { + saveLocation = sweep_location; // save in the same folder + } else { + saveLocation+="/"; + if ( ! boost::filesystem::exists( saveLocation ) ) + { + if (!boost::filesystem::create_directory(saveLocation)) + { + ROS_ERROR_STREAM("Could not create folder where to save calibration data "+saveLocation); + //as->setAborted(res,"Could not create folder where to save calibration data "+saveLocation); + return ""; + } + } + } + ROS_INFO_STREAM("The registered sweeps will be saved at: "<initializeCamera(534.191590, 534.016892,315.622746, 238.568515, 640, 480); +//exit(0); + // initialize camera parameters from the sweep + /* + if (matchingObservations.size()){ + SemanticRoom aRoom = SemanticRoomXMLParser::loadRoomFromXML(matchingObservations[0],true); + if (aRoom.getIntermediateCloudCameraParameters().size()){ + image_geometry::PinholeCameraModel aCameraModel = aRoom.getIntermediateCloudCameraParameters()[0]; + rc->initializeCamera(aCameraModel.fx(), aCameraModel.fy(), aCameraModel.cx(), aCameraModel.cy(), aCameraModel.fullResolution().width, aCameraModel.fullResolution().height); + } else { + // no camera parameters saved with the sweep -> initialize optimizer with default parameters + rc->initializeCamera(540.0, 540.0,319.5, 219.5, 640, 480); + } + }*/ + + + for (size_t i=0; i aRoom = SemanticRoomXMLParser::loadRoomFromXML(matchingObservations[i],true); + if (aRoom.m_SweepParameters != complete_sweep_parameters){ + ROS_INFO_STREAM("Skipping "< features = semantic_map_registration_features::loadRegistrationFeaturesFromSingleSweep(matchingObservations[i], false); + if (features.size() == 0) + { + // recompute orb + + unsigned found = matchingObservations[i].find_last_of("/"); + std::string base_path = matchingObservations[i].substr(0,found+1); + RegistrationFeatures reg(false); + reg.saveOrbFeatures(aRoom,base_path); + } + rc->addToTrainingORBFeatures(matchingObservations[i]); + } + + // perform calibration + std::vector cameraPoses = rc->train(); + std::vector registeredPoses; + + for (auto eigenPose : cameraPoses) + { + tf::StampedTransform tfStamped; + tfStamped.frame_id_ = "temp"; + tfStamped.child_frame_id_ = "temp"; + tf::Transform tfTr; + const Eigen::Affine3d eigenTr(eigenPose.cast()); + tf::transformEigenToTF(eigenTr, tfTr); + tfStamped.setOrigin(tfTr.getOrigin()); + tfStamped.setBasis(tfTr.getBasis()); + registeredPoses.push_back(tfStamped); + } + std::string registeredPosesFile = semantic_map_registration_transforms::saveRegistrationTransforms(registeredPoses); + registeredPoses.clear(); + registeredPoses = semantic_map_registration_transforms::loadRegistrationTransforms(registeredPosesFile); + ROS_INFO_STREAM("Calibration poses saved at: "<poses; + unsigned int x,y; + std::string rawPosesFile = semantic_map_registration_transforms::saveRegistrationTransforms(rawPoses, rc->todox,rc->todoy); + ROS_INFO_STREAM("Raw calibration data saved at: "<camera->fx, 0.0, rc->camera->cx, 0.0, 0.0, rc->camera->fy, rc->camera->cy, 0.0,0.0, 0.0, 1.0,0.0}; + camInfo.D = {0,0,0,0,0}; + image_geometry::PinholeCameraModel aCameraModel; + aCameraModel.fromCameraInfo(camInfo); + + std::string camParamsFile = semantic_map_registration_transforms::saveCameraParameters(aCameraModel); + ROS_INFO_STREAM("Camera parameters saved at: "< reg_parser(saveLocation); + + for (auto usedObs : matchingObservations) + { + SemanticRoom aRoom = SemanticRoomXMLParser::loadRoomFromXML(usedObs,true); + auto origTransforms = aRoom.getIntermediateCloudTransforms(); + aRoom.clearIntermediateCloudRegisteredTransforms(); + aRoom.clearIntermediateCloudCameraParametersCorrected(); + + std::vector corresponding_registeredPoses; + corresponding_registeredPoses = semantic_map_registration_transforms::loadCorrespondingRegistrationTransforms(aRoom.m_SweepParameters); + ROS_INFO_STREAM("Corresponing registered poses "<(aRoom); + semantic_map_room_utilities::rebuildRegisteredCloud(aRoom); + // transform to global frame of reference + tf::StampedTransform origin = origTransforms[0]; + CloudPtr completeCloud = aRoom.getCompleteRoomCloud(); + pcl_ros::transformPointCloud(*completeCloud, *completeCloud,origin); + aRoom.setCompleteRoomCloud(completeCloud); + string room_path = reg_parser.saveRoomAsXML(aRoom); + ROS_INFO_STREAM("..done"); + // recompute ORB features + unsigned found = room_path.find_last_of("/"); + std::string base_path = room_path.substr(0,found+1); + RegistrationFeatures reg(false); + reg.saveOrbFeatures(aRoom,base_path); + } + delete rc; + return saveLocation; +} + void execute(const calibrate_sweeps::CalibrateSweepsGoalConstPtr& goal, Server* as) { ROS_INFO_STREAM("Received calibrate message. Min/max sweeps: "<min_num_sweeps<<" "<max_num_sweeps); calibrate_sweeps::CalibrateSweepsResult res; - + res.calibration_file = runCalibration(goal->min_num_sweeps, goal->max_num_sweeps, goal->sweep_location, goal->save_location);//registeredPosesFile; + + if (res.calibration_file.compare("") != 0){ + as->setAborted(res,""); + }else{ + as->setSucceeded(res,"Done"); + } +/* + ROS_INFO_STREAM("Received calibrate message. Min/max sweeps: "<min_num_sweeps<<" "<max_num_sweeps); + calibrate_sweeps::CalibrateSweepsResult res; SweepParameters complete_sweep_parameters (-160, 20, 160, -30, 30, 30); ROS_INFO_STREAM("Calibrating using sweeps with paramters "<initializeCamera(534.191590, 534.016892,315.622746, 238.568515, 640, 480); //exit(0); // initialize camera parameters from the sweep - /* - if (matchingObservations.size()){ - SemanticRoom aRoom = SemanticRoomXMLParser::loadRoomFromXML(matchingObservations[0],true); - if (aRoom.getIntermediateCloudCameraParameters().size()){ - image_geometry::PinholeCameraModel aCameraModel = aRoom.getIntermediateCloudCameraParameters()[0]; - rc->initializeCamera(aCameraModel.fx(), aCameraModel.fy(), aCameraModel.cx(), aCameraModel.cy(), aCameraModel.fullResolution().width, aCameraModel.fullResolution().height); - } else { - // no camera parameters saved with the sweep -> initialize optimizer with default parameters - rc->initializeCamera(540.0, 540.0,319.5, 219.5, 640, 480); - } - }*/ + +// if (matchingObservations.size()){ +// SemanticRoom aRoom = SemanticRoomXMLParser::loadRoomFromXML(matchingObservations[0],true); +// if (aRoom.getIntermediateCloudCameraParameters().size()){ +// image_geometry::PinholeCameraModel aCameraModel = aRoom.getIntermediateCloudCameraParameters()[0]; +// rc->initializeCamera(aCameraModel.fx(), aCameraModel.fy(), aCameraModel.cx(), aCameraModel.cy(), aCameraModel.fullResolution().width, aCameraModel.fullResolution().height); +// } else { +// // no camera parameters saved with the sweep -> initialize optimizer with default parameters +// rc->initializeCamera(540.0, 540.0,319.5, 219.5, 640, 480); +// } +// } for (size_t i=0; imax_num_sweeps && itodox,rc->todoy); ROS_INFO_STREAM("Raw calibration data saved at: "<setSucceeded(res,"Done"); + */ +} + +void runCallback(const std_msgs::String::ConstPtr& msg){ + runCalibration(); } int main(int argc, char** argv) @@ -251,6 +483,7 @@ int main(int argc, char** argv) Server server(n, "calibrate_sweeps", boost::bind(&execute, _1, &server), false); ROS_INFO_STREAM("Calibrate sweep action server initialized"); server.start(); + ros::Subscriber sub = n.subscribe("calibrate_sweeps_action_server/run", 1000, runCallback); ros::spin(); return 0; } From b53622884ad4516055b4712f5d821e2a1cc29ac8 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 11 Oct 2016 17:03:39 +0200 Subject: [PATCH 164/255] ... --- .../metaroom_xml_parser/load_utilities.hpp | 2 +- quasimodo/quasimodo_brain/src/Util/Util.cpp | 91 +++++++++ quasimodo/quasimodo_brain/src/Util/Util.h | 4 + .../metaroom_additional_view_processing.cpp | 178 +++++++----------- .../quasimodo_models/src/core/RGBDFrame.cpp | 16 +- .../src/modelupdater/ModelUpdater.cpp | 33 ++-- .../src/registration/MassRegistration.cpp | 2 +- .../src/registration/MassRegistrationPPR2.cpp | 31 +-- .../DistanceWeightFunction2PPR2.cpp | 82 ++------ 9 files changed, 218 insertions(+), 221 deletions(-) diff --git a/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp b/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp index e07d1061..08709e28 100644 --- a/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp +++ b/metaroom_xml_parser/include/metaroom_xml_parser/load_utilities.hpp @@ -404,7 +404,7 @@ std::vector> loadAllDynamicObjectsFromSingleSweep(s if (verbose){ std::cout<<"Found "<submodels.push_back(sweep); + model->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + + + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + printf("folder: %s\n",sweep_folder.c_str()); + + + std::vector> objects = semantic_map_load_utilties::loadAllDynamicObjectsFromSingleSweep(sweep_folder+"room.xml"); + + for (auto object : objects){ + if (!object.objectScanIndices.size()){continue;} + + printf("%i %i %i\n",int(object.vAdditionalViews.size()),int(object.vAdditionalViewsTransformsRegistered.size()),int(object.vAdditionalViewsTransforms.size())); + for (unsigned int i = 0; i < object.vAdditionalViews.size(); i ++){ + pcl::PointCloud::Ptr cloud = object.vAdditionalViews[i]; + + cv::Mat rgb; + rgb.create(cloud->height,cloud->width,CV_8UC3); + unsigned char * rgbdata = (unsigned char *)rgb.data; + + cv::Mat depth; + depth.create(cloud->height,cloud->width,CV_16UC1); + unsigned short * depthdata = (unsigned short *)depth.data; + + unsigned int nr_data = cloud->height * cloud->width; + for(unsigned int j = 0; j < nr_data; j++){ + pcl::PointXYZRGB p = cloud->points[j]; + rgbdata[3*j+0] = p.b; + rgbdata[3*j+1] = p.g; + rgbdata[3*j+2] = p.r; + depthdata[j] = short(5000.0 * p.z); + } + + cv::Mat fullmask; + fullmask.create(cloud->height,cloud->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)fullmask.data; + for(int j = 0; j < nr_data; j++){maskdata[j] = 255;} + + reglib::Camera * cam = sweep->frames.front()->camera->clone(); + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth,0, getMat(object.vAdditionalViewsTransforms[i])); + model->frames.push_back(frame); + if(model->relativeposes.size() == 0){ + model->relativeposes.push_back(Eigen::Matrix4d::Identity());//current_room_frames.front()->pose.inverse() * frame->pose); + }else{ + model->relativeposes.push_back(model->frames.front()->pose.inverse() * frame->pose); + } + model->modelmasks.push_back(new reglib::ModelMask(fullmask)); + } + } + + +// if(model->relativeposes.size() == 0){ +// model->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); +// }else{ +// model->submodels_relativeposes.push_back(model->relativeposes.front().inverse() * sweep->frames.front()->pose); +// } + + + return model; +} + +std::vector getRegisteredViewPoses(std::string poses_file, int no_transforms){ + std::vector toRet; + ifstream in(poses_file); + if (!in.is_open()){ + cout<<"ERROR: cannot find poses file "<> temp; + transform(j,k) = temp; + } + } + toRet.push_back(transform); + } + return toRet; +} + double getTime(){ struct timeval start1; gettimeofday(&start1, NULL); diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index 53291c63..f4fe86b4 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -31,6 +31,7 @@ #include "core/RGBDFrame.h" #include "metaroom_xml_parser/simple_xml_parser.h" +#include #include #include @@ -38,6 +39,9 @@ namespace quasimodo_brain { +std::vector getRegisteredViewPosesFromFile(std::string poses_file, int no_transforms); +reglib::Model * loadFromRaresFormat(std::string path); + double getTime(); reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index db141ef8..3109fee4 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -89,6 +89,7 @@ #include + std::vector< ros::ServiceServer > m_DynamicObjectsServiceServers; std::vector< ros::ServiceServer > m_GetDynamicObjectServiceServers; @@ -129,6 +130,7 @@ std::vector< ros::Publisher > model_pubs; std::string posepath = "testposes.xml"; std::vector sweepPoses; +reglib::Camera * basecam; bool testDynamicObjectServiceCallback(std::string path); @@ -464,6 +466,28 @@ void readViewXML(std::string xmlFile, std::vector & frames, delete xmlReader; } + +void setBaseSweep(std::string path){ + printf("setBaseSweep(%s)\n",path.c_str()); + SimpleXMLParser parser; + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(path);//, std::vector(),false,false); +printf("loaded room data\n"); + image_geometry::PinholeCameraModel baseCameraModel = roomData.vIntermediateRoomCloudCamParamsCorrected.front(); + if(basecam != 0){delete basecam;} + basecam = new reglib::Camera(); + basecam->fx = baseCameraModel.fx(); + basecam->fy = baseCameraModel.fy(); + basecam->cx = baseCameraModel.cx(); + basecam->cy = baseCameraModel.cy(); + + sweepPoses.clear(); + for (size_t i=0; iframes.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; sweep->relativeposes[i] = sweepPoses[i]; + if(basecam != 0){ + delete sweep->frames[i]->camera; + sweep->frames[i]->camera = basecam->clone(); + } } std::vector frames; @@ -561,11 +589,11 @@ reglib::Model * processAV(std::string path){ if(frames.size() > 0){ reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.15); - bgmassreg->timeout = 20; + bgmassreg->timeout = 300; bgmassreg->viewer = viewer; bgmassreg->use_surface = true; bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = 0; + bgmassreg->visualizationLvl = visualization_lvl; bgmassreg->maskstep = 5; bgmassreg->nomaskstep = 5; bgmassreg->nomask = true; @@ -576,6 +604,23 @@ reglib::Model * processAV(std::string path){ reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); delete bgmassreg; +// reglib::MassRegistrationPPR2 * bgmassreg2 = new reglib::MassRegistrationPPR2(0.01); + +// bgmassreg2->timeout = 300; +// bgmassreg2->viewer = viewer; +// bgmassreg2->use_surface = true; +// bgmassreg2->use_depthedge = false; +// bgmassreg2->visualizationLvl = visualization_lvl; +// bgmassreg2->maskstep = 2; +// bgmassreg2->nomaskstep = 2; +// bgmassreg2->nomask = true; +// bgmassreg2->stopval = 0.0005; +// bgmassreg2->addModel(sweep); +// bgmassreg2->setData(frames,masks); + +// reglib::MassFusionResults bgmfr2 = bgmassreg2->getTransforms(bgmfr.poses); +// delete bgmassreg2; + for(unsigned int i = 0; i < frames.size(); i++){ frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; } @@ -593,6 +638,7 @@ reglib::Model * processAV(std::string path){ delete reg; delete mu; delete sweep; + delete basecam; return fullmodel; } @@ -1060,14 +1106,14 @@ void trainMetaroom(std::string path){ bgmassreg->viewer = viewer; bgmassreg->use_surface = true; bgmassreg->use_depthedge = true; - bgmassreg->visualizationLvl = 0; + bgmassreg->visualizationLvl = visualization_lvl;//0; bgmassreg->maskstep = 5; bgmassreg->nomaskstep = 5; bgmassreg->nomask = true; bgmassreg->stopval = 0.0005; bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); - +exit(0); savePoses(overall_folder+"/"+posepath,bgmfr.poses,17); fullmodel->fullDelete(); delete fullmodel; @@ -1167,13 +1213,6 @@ void sendMetaroomToServer(std::string path){ void sendCallback(const std_msgs::String::ConstPtr& msg){sendMetaroomToServer(msg->data);} -//string waypoint_id -//--- -//string[] object_id -//sensor_msgs/PointCloud2[] objects -//geometry_msgs/Point[] centroids - - bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res){ printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n"); std::string current_waypointid = req.waypoint_id; @@ -1267,18 +1306,8 @@ bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObj } bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res){ - // string waypoint_id - // string object_id - // --- - // sensor_msgs/PointCloud2 object_cloud - // int32[] object_mask - // geometry_msgs/Transform transform_to_map - // int32 pan_angle - // int32 tilt_angle - std::string current_waypointid = req.waypoint_id; - if(overall_folder.back() == '/'){overall_folder.pop_back();} SimpleXMLParser parser; @@ -1291,7 +1320,6 @@ bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDyn } if(prevind == -1){return false;} std::string path = sweep_xmls[prevind]; - //processMetaroom(path); printf("path: %s\n",path.c_str()); int slash_pos = path.find_last_of("/"); @@ -1300,101 +1328,22 @@ bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDyn printf("sweep_folder: %s\n",sweep_folder.c_str()); printf("req.object_id: %s\n",req.object_id.c_str()); char buf [1024]; - //sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),atoi(req.object_id.c_str()); - //sprintf(buf,"%s/dynamic_object%10.10i.xml",sweep_folder.c_str(),atoi(req.object_id.c_str()); std::string fullpath = std::string(buf); -// best_transform = roomTransform.inverse() * best_transform; -// const Eigen::Affine3d eigenTr(best_transform.cast()); -// tf::transformEigenToTF(eigenTr,returned_object.transform_to_map); -// returned_object.object_cloud = CloudPtr(new Cloud()); -// *returned_object.object_cloud = *allClouds[best_index]; -// // pcl::transformPointCloud (*returned_object.object_cloud, *returned_object.object_cloud, best_transform); // transform to map frame - -// returned_object.object_indices.insert(returned_object.object_indices.begin(), src_indices.begin(),src_indices.end()); -// returned_object.object_mask = cluster_image; - -// // save mask and indices -// if (m_bSaveMask) { -// // find observation folder -// int slash_pos = observation_xml.find_last_of("/"); -// std::string observation_folder = observation_xml.substr(0, slash_pos); -// std::string mask_image = observation_folder + "/" + object_id + "_mask.jpg"; -// cv::imwrite(mask_image, cluster_image); -// std::string mask_indices = observation_folder + "/" + object_id + "_mask.txt"; -// ofstream mask_indices_os; -// mask_indices_os.open(mask_indices); -// for (int index : returned_object.object_indices){ -// mask_indices_os << index<<" "; -// } -// mask_indices_os.close(); -// ROS_INFO_STREAM("Object mask saved at: "< currentObjects; - bool objects_found = updateObjectsAtWaypoint(req.waypoint_id); - if (!objects_found){return true;} - - GetObjStruct object; - bool found =returnObjectMask(req.waypoint_id, req.object_id,m_waypointToSweepFileMap[req.waypoint_id], object); - if (!found){ - ROS_ERROR_STREAM("Could not compute mask for object id "< if not, add the intermediate cloud & its transform - // if (!m_objectTracked->m_vAdditionalViews.size()){ - // m_objectTracked->addAdditionalView(object.object_cloud); - // m_objectTracked->addAdditionalViewTransform(object.transform_to_map); - // } + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(path); + for (auto sweep_xml : sweep_xmls) { + printf("sweep_xml: %s\n",sweep_xml.c_str()); + processMetaroom(sweep_xml); + ///reglib::Model * fullmodel = processAV(sweep_xml); } - - return true; - -*/ + return false; +} bool testDynamicObjectServiceCallback(std::string path){ printf("bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDynamicObjectServiceResponse &res)\n"); @@ -1457,6 +1406,7 @@ int main(int argc, char** argv){ std::vector trainMetarooms; std::vector sendMetaroomToServers; std::vector processMetarooms; + std::vector raresfiles; int inputstate = 0; for(int i = 1; i < argc;i++){ @@ -1483,6 +1433,8 @@ int main(int argc, char** argv){ else if(std::string(argv[i]).compare("-DynamicObjectsService") == 0){ inputstate = 12;} else if(std::string(argv[i]).compare("-GetDynamicObjectService") == 0){ inputstate = 13;} else if(std::string(argv[i]).compare("-statusmsg") == 0){ inputstate = 14;} + else if(std::string(argv[i]).compare("-files") == 0){ inputstate = 15;} + else if(std::string(argv[i]).compare("-baseSweep") == 0){ inputstate = 16;} else if(std::string(argv[i]).compare("-once") == 0){ once = true;} else if(std::string(argv[i]).compare("-nobase") == 0){ baseSetting = false;} else if(inputstate == 0){ @@ -1515,7 +1467,12 @@ int main(int argc, char** argv){ m_GetDynamicObjectServiceServers.push_back(n.advertiseService(std::string(argv[i]), getDynamicObjectServiceCallback)); }else if(inputstate == 14){ m_PublisherStatuss.push_back(n.advertise(std::string(argv[i]), 1000)); + }else if(inputstate == 15){ + raresfiles.push_back(std::string(argv[i])); + }else if(inputstate == 16){ + setBaseSweep(std::string(argv[i])); } + } if(baseSetting){ @@ -1554,6 +1511,7 @@ int main(int argc, char** argv){ printf("overall_folder: %s\n",overall_folder.c_str()); + for(unsigned int i = 0; i < raresfiles.size(); i++){ segmentRaresFiles( raresfiles[i]);} for(unsigned int i = 0; i < trainMetarooms.size(); i++){ trainMetaroom( trainMetarooms[i]);} for(unsigned int i = 0; i < processMetarooms.size(); i++){ processMetaroom( processMetarooms[i]);} for(unsigned int i = 0; i < sendMetaroomToServers.size(); i++){ sendMetaroomToServer( sendMetaroomToServers[i]);} diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 628a9069..53afeace 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -938,16 +938,26 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt // cv::namedWindow( "blur", cv::WINDOW_AUTOSIZE ); cv::imshow( "blur", blur_rgb); // //cv::namedWindow( "maximas", cv::WINDOW_AUTOSIZE ); cv::imshow( "blur", blur_rgb); // cv::waitKey(0); -// // cv::namedWindow( "det", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det); -// // cv::waitKey(0); + // cv::namedWindow( "det", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det); + // cv::waitKey(0); int dilation_size = 4; cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) ); + + unsigned char * det_dilatedata = (unsigned char*)det_dilate.data; + depthedges.create(height,width,CV_8UC1); unsigned char * depthedgesdata = (unsigned char *)depthedges.data; - for(int i = 0; i < width*height; i++){depthedgesdata[i] = 0;} + for(int i = 0; i < width*height; i++){ + //depthedgesdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5) || (cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5)); + depthedgesdata[i] = 255*(((cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5)) && (det_dilatedata[i] == 0)); + //depthedgesdata[i] = 0; + } + + //cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges); + //cv::waitKey(0); if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 4cf2b06b..8efeee33 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1239,9 +1239,9 @@ cv::Mat getImageFromArray(unsigned int width, unsigned int height, double * arr) } } - cv::namedWindow( "getImageFromArray", cv::WINDOW_AUTOSIZE ); - cv::imshow( "getImageFromArray",m); - cv::waitKey(0); +// cv::namedWindow( "getImageFromArray", cv::WINDOW_AUTOSIZE ); +// cv::imshow( "getImageFromArray",m); +// cv::waitKey(0); return m; } @@ -1634,7 +1634,7 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int b DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); func->zeromean = true; - func->maxp = 0.9999; + func->maxp = 0.99999; func->startreg = 0.5; func->debugg_print = false; //func->bidir = true; @@ -1857,9 +1857,9 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int b edgesdata[3*i+2] = dyc[i]; } - // cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); - // cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); - // cv::waitKey(0); + cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); + cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); + cv::waitKey(0); return probs2; } @@ -2100,11 +2100,12 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dfunc = dfuncTMP; dfuncTMP->refine_mean = true; dfuncTMP->zeromean = false; - //dfuncTMP->startreg = 0.0005; - dfuncTMP->startreg = 0.00001; + //dfuncTMP->startreg = 0.001; + dfuncTMP->startreg = 0.0000001; dfuncTMP->debugg_print = false; dfuncTMP->bidir = true; //dfuncTMP->bidir = false; + //dfuncTMP->maxp = 0.9; dfuncTMP->maxp = 0.9; dfuncTMP->maxd = 0.5; dfuncTMP->histogram_size = 1000; @@ -2155,6 +2156,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s // printf("adding constraints\n"); for(unsigned int i = 0; i < frames.size(); i++){ + printf("currently workin on frame %i\n",i); int offset = offsets[i]; RGBDFrame * frame = frames[i]; startTime = getTime(); @@ -2283,7 +2285,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s printf("total_Dynw = %5.5fs\n", total_Dynw); -// printf("running inference\n"); + printf("setup inference\n"); long interframeConnections = 0; for(unsigned int i = 0; i < interframe_connectionId.size();i++){ @@ -2343,9 +2345,10 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for(unsigned int i = 0; i < interframe_connectionId.size();i++){ for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ double weight = interframe_connectionStrength[i][j]; - g -> add_edge( i, interframe_connectionId[i][j], weight, weight ); + //g -> add_edge( i, interframe_connectionId[i][j], weight, weight ); } } + printf("run inference\n"); g -> maxflow(); int dynamic_label = 1; @@ -2468,7 +2471,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ if(labels[interframe_connectionId[i][j]] == labels[i]){ double weight = interframe_connectionStrength[i][j]; - dynamic_g -> add_edge( stat_ind[i], stat_ind[ interframe_connectionId[i][j] ], weight, weight ); + //dynamic_g -> add_edge( stat_ind[i], stat_ind[ interframe_connectionId[i][j] ], weight, weight ); } } } @@ -3505,9 +3508,9 @@ std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eig unsigned char * internaldata = (unsigned char *)(internalmask.data); for(unsigned int i = 0; i < width*height;i++){internaldata[i] = 255.0*(g->what_segment(i) == gc::Graph::SOURCE);} - cv::imshow( "rgb", cf[i]->rgb ); - cv::imshow( "internalmask", internalmask ); - cv::waitKey(30); +// cv::imshow( "rgb", cf[i]->rgb ); +// cv::imshow( "internalmask", internalmask ); +// cv::waitKey(30); newmasks.push_back(internalmask); //Time to compute external masks... diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp index cc2344ba..54bc38a7 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp @@ -119,7 +119,7 @@ void MassRegistration::show(std::vector Xv, bool save, std::str cloud->points.push_back(p); } - printf("cloud->points: %i\n",cloud->points.size()); + //printf("cloud->points: %i\n",cloud->points.size()); char buf [1024]; sprintf(buf,"cloud%i",xi); diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index a8f76168..65fd7853 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -534,23 +534,7 @@ void MassRegistrationPPR2::setData(std::vector frames_,std::vector 0){ -// for(unsigned long i = 0; i < arraypoints.size(); i++){delete[] arraypoints[i];} -// arraypoints.resize(0); -// } - -// if(a3dv.size() > 0){ -// for(unsigned long i = 0; i < a3dv.size(); i++){delete a3dv[i];} -// a3dv.resize(0); -// } - -// if(trees3d.size() > 0){ -// for(unsigned long i = 0; i < trees3d.size(); i++){delete trees3d[i];} -// trees3d.resize(0); -// } - for(unsigned long i = 0; i < nr_frames; i++){addData(frames_[i], mmasks_[i]);} - //printf("total load time: %5.5f\n",getTime()-total_load_time_start); } double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ap, double * ap, std::vector & matchid, std::vector & matchdist, Tree3d * t3d, double & new_good_rematches, double & new_total_rematches){ @@ -1448,7 +1432,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ } } - //printf("depthedge_count: %i\n",depthedge_count); + printf("depthedge_count: %i\n",depthedge_count); if(depthedge_count > 10){ double * depthedge_ap = new double[3*depthedge_count]; double * depthedge_ai = new double[3*depthedge_count]; @@ -3071,6 +3055,7 @@ void MassRegistrationPPR2::showEdges(std::vector poses){ p.r = b; cloud->points.push_back(p); } + printf("showEdges %i -> %i\n",i,cloud->points.size()); } if(cloud->points.size() > 0){ viewer->removeAllPointClouds(); @@ -3135,7 +3120,6 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectorreset(); @@ -3204,7 +3188,7 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector 0){ printf("funcupdate: %i rematching: %i lala: %i\n",funcupdate,rematching,lala); printf("total_time: %5.5f\n",getTime()-total_time_start); @@ -3247,13 +3231,8 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectorcomputeModel(all_residuals); - } - - if(use_depthedge){ - depthedge_func->computeModel(depthedge_all_residuals); - } + if(use_surface){ func->computeModel(all_residuals);} + if(use_depthedge){ depthedge_func->computeModel(depthedge_all_residuals);} //printf("surface reg: %5.5f noise: %5.5f\n",func->regularization,func->noiseval); //printf("edge: reg: %5.5f noise: %5.5f\n",depthedge_func->regularization,depthedge_func->noiseval); diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp index e309d404..4e582248 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp @@ -668,6 +668,8 @@ void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ const unsigned int nr_data = mat.cols(); const int nr_dim = mat.rows(); +double startTime = getCurrentTime3(); +//printf("\n################################################### ITER:%i ############################################################\n",iter); if(debugg_print){printf("\n################################################### ITER:%i ############################################################\n",iter);} if(!fixed_histogram_size){ if(first){ @@ -713,7 +715,7 @@ void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ if(debugg_print){printf("nr_inside: %f histogram_size: %i blurval: %f\n",nr_inside,histogram_size,blurval);} } } - +//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3(); histogram_mul = double(histogram_size)/maxd; histogram_mul2 = double(histogram_size)/(maxd-mind); // if(bidir){ histogram_mul2 = double(histogram_size)/(2.0*maxd);} @@ -732,46 +734,12 @@ void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ } } } - +//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3(); //double ind = getInd(d,debugg); -// if(!fixed_histogram_size){ -// for(unsigned int j = 0; j < nr_data; j++){ -// for(int k = 0; k < nr_dim; k++){ -// double ind = fabs(mat(k,j))*histogram_mul; -// double w2 = ind-int(ind); -// double w1 = 1-w2; -// if(ind >= 0 && (ind+0.5) < histogram_size){histogram[int(ind+0.5)]++;} -// } -// } -// histogram[0]*=2; -//}else if(bidir){ -// for(unsigned int j = 0; j < histogram_size; j++){histogram[j] = 0;} -// for(unsigned int j = 0; j < nr_data; j++){ -// for(int k = 0; k < nr_dim; k++){ -// double ind = double(histogram_size)*(mat(k,j)+maxd)/(2.0*maxd); -// if(ind >= 0 && (ind+0.00001) < histogram_size){ -// histogram[int(ind+0.00001)]++; -// } -// } -// } -//}else{ -// for(unsigned int j = 0; j < nr_data; j++){ -// for(int k = 0; k < nr_dim; k++){ -// double ind = fabs(mat(k,j))*histogram_mul; -// double w2 = ind-int(ind); -// double w1 = 1-w2; -// if(ind >= 0 && (ind+0.00001) < histogram_size){ -// histogram[int(ind+0.00001)]++; -// } -// //if(j % 5000 == 0){printf("%i %i -> r:%f ind: %f \n",j,k,mat(k,j),ind);} -// } -// } -//} - start_time = getCurrentTime3(); blurHistogram3(blur_histogram,histogram,blurval,histogram_size,debugg_print); - +//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3(); // stdval2 = 0; // Gaussian3 g = getModel(stdval, blur_histogram, uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); // //Gaussian3 g2 = getModel(stdval2,histogram, uniform_bias,refine_mean,refine_mul,refine_std,nr_refineiters,costpen,zeromean); @@ -814,7 +782,7 @@ void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ // gd->train(blur_histogram,histogram_size); // gd->print(); // delete gd; - +//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3(); g.stdval = std::max(0.0001*double(histogram_size)/(maxd),std::min(g.stdval,maxnoise*double(histogram_size)/maxd)); //noiseval = maxd*g.stdval/float(histogram_size); @@ -836,7 +804,7 @@ void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ if(compute_infront){ for(int k = 0; k < histogram_size; k++){noisecdf[k] = g.getcdf(k);} } - +//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3(); // std::vector new_histogram; // if(rescaling){ // new_histogram.resize(blur_histogram.size()); @@ -858,20 +826,16 @@ void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ if(max_under_mean && k < g.mean){ prob[k] = maxp; } else{ double hs = blur_histogram[k]+1; -// if(rescaling){hs = new_histogram[k]+1;} prob[k] = std::min(maxp , noise[k]/hs);//never fully trust any data infront[k] = (1-prob[k])*noisecdf[k]; } } - +//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3(); nr_inliers = 0; for(unsigned int j = 0; j < nr_data; j++){ float inl = 1; float ninl = 1; for(int k = 0; k < nr_dim; k++){ - //double ind = getInd(mat(k,j));//fabs(mat(k,j))*histogram_mul; - //float p = 0; - //if(ind >= 0 && (ind+0.5) < histogram_size){p = prob[int(ind+0.5)];} float p = getProb(mat(k,j)); inl *= p; ninl *= 1.0-p; @@ -879,7 +843,7 @@ void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ double d = inl / (inl+ninl); nr_inliers += d; } - +//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3();//SLOW if(debugg_print){printf("hist = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(histogram[k]));} printf("];\n");} if(debugg_print){printf("noise = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%i ", int(noise[k]));} printf("];\n");} if(debugg_print){printf("noisecdf = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",int(noisecdf[k]));} printf("];\n");} @@ -909,22 +873,23 @@ void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ if(next_maxd == next_mind){ return;} double overlap = 0; - if(next_maxd < maxd && next_mind > mind){overlap = next_maxd-next_mind;} - if(next_maxd > maxd && next_mind < mind){overlap = maxd- mind;} - if(next_maxd < maxd && next_mind < mind){overlap = next_maxd- mind;} - if(next_maxd > maxd && next_mind > mind){overlap = maxd-next_mind;} + if(next_maxd <= maxd && next_mind >= mind){overlap = next_maxd -next_mind;} + if(next_maxd >= maxd && next_mind <= mind){overlap = maxd - mind;} + if(next_maxd <= maxd && next_mind <= mind){overlap = next_maxd - mind;} + if(next_maxd >= maxd && next_mind >= mind){overlap = maxd -next_mind;} double ratio = 2*overlap/(maxd+next_maxd-mind-next_mind); double newlogdiff = log(ratio); -// if(bidir){ + //if(bidir){ // printf("meanoffset: %f stdval2: %f stdval: %f regularization: %f\n",meanval2,stdval2,noiseval,regularization); // printf("mind: %5.5f maxd: %5.5f noise: %5.5f noise+reg: %5.5f\n",mind,maxd,noiseval,getNoise()); // printf("next_maxd: %5.5f maxd: %5.5f\n",next_maxd,maxd); // printf("next_mind: %5.5f mind: %5.5f\n",next_mind,mind); // printf("logdiff: %5.5f\n",logdiff); // printf("newlogdiff: %5.5f\n",newlogdiff); -// } +// printf("ratio: %5.5f = %5.5f / %5.5f\n",ratio,2*overlap,(maxd+next_maxd-mind-next_mind) ); + //} //double next_maxd = meanval2 + (stdval2 + regularization)*target_length; @@ -937,7 +902,7 @@ void DistanceWeightFunction2PPR2::computeModel(MatrixXd mat){ return; }else{iter = 0;} } - +//printf("time taken to %5.5i: %10.10f\n",__LINE__,getCurrentTime3()-startTime); startTime = getCurrentTime3(); //if(bidir){exit(0);} } @@ -951,19 +916,6 @@ VectorXd DistanceWeightFunction2PPR2::getProbs(MatrixXd mat){ float inl = 1; float ninl = 1; for(int k = 0; k < nr_dim; k++){ -// double ind; -// float p = 0; -// if(bidir){ -// ind = double(histogram_size)*(mat(k,j)+maxd)/(2.0*maxd); -// }else{ -// ind = fabs(mat(k,j))*histogram_mul; -// } -// double w2 = ind-int(ind); -// double w1 = 1-w2; -// if(ind >= 0 && (ind+0.5) < histogram_size){ -// if(interp){ p = prob[int(ind)]*w1 + prob[int(ind+1)]*w2; -// }else{ p = prob[int(ind+0.5)];} -// } float p = getProb(mat(k,j)); inl *= p; ninl *= 1.0-p; From 51f7f76f5fc0be198efe6caa314cb05ea2c5955f Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 11 Oct 2016 17:10:05 +0200 Subject: [PATCH 165/255] ... --- .../quasimodo_models/include/core/RGBDFrame.h | 2 ++ .../quasimodo_models/src/core/RGBDFrame.cpp | 17 +++++++++++++++++ .../src/modelupdater/ModelUpdater.cpp | 6 +++--- 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 41e6a0df..2fb57440 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -66,6 +66,8 @@ namespace reglib std::vector getReprojections(std::vector & spvec, Eigen::Matrix4d cp, bool * maskvec, bool useDet = true); std::vector getSuperPoints(Eigen::Matrix4d cp = Eigen::Matrix4d::Identity(), unsigned int step = 1, bool zeroinclude = true); + + std::vector< std::vector > getImageProbs(); }; } diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 53afeace..9e20e0d0 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -1437,5 +1437,22 @@ std::vector RGBDFrame::getSuperPoints(Eigen::Matrix4d cp, unsigned i return ret; } +std::vector< std::vector > RGBDFrame::getImageProbs(){ + + unsigned int width = camera->width; + unsigned int height = camera->height; + + std::vector dxc; + dxc.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0;} + + std::vector dyc; + dyc.resize(width*height); + for(unsigned int i = 0; i < width*height;i++){dyc[i] = 0;} + + std::vector< std::vector > probs2; + probs2.push_back(dxc); + probs2.push_back(dyc); +} } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 8efeee33..9d740102 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1857,9 +1857,9 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int b edgesdata[3*i+2] = dyc[i]; } - cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); - cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); - cv::waitKey(0); +// cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); +// cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); +// cv::waitKey(0); return probs2; } From f27ecb6bb5ecbbdada203c59e40c08a9572f662c Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 14 Oct 2016 17:29:30 +0200 Subject: [PATCH 166/255] ... --- .../metaroom_additional_view_processing.cpp | 182 +- quasimodo/quasimodo_models/CMakeLists.txt | 2 +- .../quasimodo_models/include/core/RGBDFrame.h | 2 +- .../include/core/ReprojectionResult.h | 3 +- .../quasimodo_models/include/core/Util.h | 18 + .../quasimodo_models/src/core/RGBDFrame.cpp | 894 ++++---- .../src/core/ReprojectionResult.cpp | 3 +- .../src/modelupdater/ModelUpdater.cpp | 1319 +++++++++--- .../src/registration/MassRegistrationPPR2.cpp | 1857 ++++++++--------- 9 files changed, 2571 insertions(+), 1709 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 3109fee4..b50a28dd 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -164,16 +164,16 @@ void remove_old_seg(std::string sweep_folder){ std::remove((sweep_folder+"/"+file).c_str()); } -// if (file.find("_object_") !=std::string::npos && file.find(".xml") !=std::string::npos){ -// printf ("removing %s\n", ent->d_name); -// std::remove((sweep_folder+"/"+file).c_str()); -// } -// if (file.find("_object_") !=std::string::npos && file.find(".pcd") !=std::string::npos){ -// printf ("maybe: removing %s\n", ent->d_name); -// printf("%i\n",file.find("_additional_view_") == std::string::npos); -// //file.find("_additional_view_") == std::string::npos; -// //std::remove((sweep_folder+"/"+file).c_str()); -// } + // if (file.find("_object_") !=std::string::npos && file.find(".xml") !=std::string::npos){ + // printf ("removing %s\n", ent->d_name); + // std::remove((sweep_folder+"/"+file).c_str()); + // } + // if (file.find("_object_") !=std::string::npos && file.find(".pcd") !=std::string::npos){ + // printf ("maybe: removing %s\n", ent->d_name); + // printf("%i\n",file.find("_additional_view_") == std::string::npos); + // //file.find("_additional_view_") == std::string::npos; + // //std::remove((sweep_folder+"/"+file).c_str()); + // } } closedir (dir); } @@ -471,7 +471,7 @@ void setBaseSweep(std::string path){ printf("setBaseSweep(%s)\n",path.c_str()); SimpleXMLParser parser; SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(path);//, std::vector(),false,false); -printf("loaded room data\n"); + printf("loaded room data\n"); image_geometry::PinholeCameraModel baseCameraModel = roomData.vIntermediateRoomCloudCamParamsCorrected.front(); if(basecam != 0){delete basecam;} basecam = new reglib::Camera(); @@ -544,7 +544,7 @@ reglib::Model * processAV(std::string path){ std::vector both_unrefined; both_unrefined.push_back(Eigen::Matrix4d::Identity()); std::vector times; - for(unsigned int i = 0; i < viewrgbs.size(); i++){ + for(unsigned int i = 0; i < 3 && i < viewrgbs.size(); i++){ printf("additional view: %i\n",i); geometry_msgs::TransformStamped msg; tf::transformStampedTFToMsg(viewtfs[i], msg); @@ -569,6 +569,102 @@ reglib::Model * processAV(std::string path){ reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m);//a.matrix()); frames.push_back(frame); + //Visualize edges... + viewer->removeAllPointClouds(); + viewer->removeAllShapes(); + std::vector< std::vector > probs = frame->getImageProbs(); + unsigned char * det_dilatedata = frame->det_dilate.data; + + // printf("saving pcd: %s\n",path.c_str()); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + + const unsigned int width = frame->camera->width; + const unsigned int height = frame->camera->height; + const double idepth = frame->camera->idepth_scale; + const double cx = frame->camera->cx; + const double cy = frame->camera->cy; + const double ifx = 1.0/frame->camera->fx; + const double ify = 1.0/frame->camera->fy; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(width*height); + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + double z = idepth*double(depthdata[ind]); + + if(z > 0){ + pcl::PointXYZRGB p; + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + p.b = 0;//rgbdata[3*ind+0]; + p.g = 0;//rgbdata[3*ind+1]; + p.r = 0;//rgbdata[3*ind+2]; + cloud->points[ind] = p; + } + } + } +viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + + for(unsigned int w = 1; w < width; w++){ + for(unsigned int h = 1; h < height;h++){ + int ind = h*width+w; + pcl::PointXYZRGB & p = cloud->points[ind]; + if(p.z > 1.5 || p.z == 0){ + p.r = 0; + p.g = 0; + p.b = 0; + }else{ + if(det_dilatedata[ind]){ + float px = 1.0-probs[0][ind]; + float py = 1.0-probs[1][ind]; + char buf [1024]; + + { + + pcl::PointXYZRGB p1 = cloud->points[ind-1]; + double d = sqrt(pow(p.x-p1.x,2)+pow(p.y-p1.y,2)+pow(p.z-p1.z,2)); + if(d > 0.2 && px > 0.5){ + printf("%i , %i :: %i -> %i\n",w,h,ind,ind-1); + sprintf(buf,"lineX_%i",ind); + viewer->addLine(p,p1,px,0,0,buf); + viewer->spin(); + } + } +// { +// pcl::PointXYZRGB p1 = cloud->points[ind-width]; +// sprintf(buf,"lineY_%i",ind); +// viewer->addLine(p,p1,0,1,0,buf); +// } + + // if(px > 0.1){ + // pcl::PointXYZRGB p1 = cloud->points[ind-1]; + // p1 = p; p1.x *= 1.1; p1.y *= 1.1; p1.z *= 1.1; + // //if(p1.z < 0.1){p1 = p; p1.x *= 1.1; p1.y *= 1.1; p1.z *= 1.1;} + + // sprintf(buf,"lineX_%i",ind); + // viewer->addLine(p,p1,1,0,0,buf); + // } + // if(py > 0.1){ + // pcl::PointXYZRGB p1 = cloud->points[ind-width]; + // p1 = p; p1.x *= 1.1; p1.y *= 1.1; p1.z *= 1.1; + // //if(p1.z < 0.1){p1 = p; p1.x *= 1.1; p1.y *= 1.1; p1.z *= 1.1;} + // sprintf(buf,"lineY_%i",ind); + // viewer->addLine(p,p1,0,1,0,buf); + // } + } + } + } + } + + + viewer->spin(); + //both_unrefined.push_back(sweep->frames.front()->pose.inverse()*a.matrix()); both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); } @@ -593,7 +689,7 @@ reglib::Model * processAV(std::string path){ bgmassreg->viewer = viewer; bgmassreg->use_surface = true; bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = visualization_lvl; + bgmassreg->visualizationLvl = 0;//visualization_lvl; bgmassreg->maskstep = 5; bgmassreg->nomaskstep = 5; bgmassreg->nomask = true; @@ -604,22 +700,22 @@ reglib::Model * processAV(std::string path){ reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); delete bgmassreg; -// reglib::MassRegistrationPPR2 * bgmassreg2 = new reglib::MassRegistrationPPR2(0.01); + // reglib::MassRegistrationPPR2 * bgmassreg2 = new reglib::MassRegistrationPPR2(0.01); -// bgmassreg2->timeout = 300; -// bgmassreg2->viewer = viewer; -// bgmassreg2->use_surface = true; -// bgmassreg2->use_depthedge = false; -// bgmassreg2->visualizationLvl = visualization_lvl; -// bgmassreg2->maskstep = 2; -// bgmassreg2->nomaskstep = 2; -// bgmassreg2->nomask = true; -// bgmassreg2->stopval = 0.0005; -// bgmassreg2->addModel(sweep); -// bgmassreg2->setData(frames,masks); + // bgmassreg2->timeout = 300; + // bgmassreg2->viewer = viewer; + // bgmassreg2->use_surface = true; + // bgmassreg2->use_depthedge = false; + // bgmassreg2->visualizationLvl = visualization_lvl; + // bgmassreg2->maskstep = 2; + // bgmassreg2->nomaskstep = 2; + // bgmassreg2->nomask = true; + // bgmassreg2->stopval = 0.0005; + // bgmassreg2->addModel(sweep); + // bgmassreg2->setData(frames,masks); -// reglib::MassFusionResults bgmfr2 = bgmassreg2->getTransforms(bgmfr.poses); -// delete bgmassreg2; + // reglib::MassFusionResults bgmfr2 = bgmassreg2->getTransforms(bgmfr.poses); + // delete bgmassreg2; for(unsigned int i = 0; i < frames.size(); i++){ frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; @@ -717,8 +813,8 @@ int processMetaroom(std::string path, bool store_old_xml = true){ int returnval = 0; printf("processing: %s\n",path.c_str()); -// testDynamicObjectServiceCallback(path); -// exit(0); + // testDynamicObjectServiceCallback(path); + // exit(0); if ( ! boost::filesystem::exists( path ) ){return 0;} @@ -747,10 +843,10 @@ int processMetaroom(std::string path, bool store_old_xml = true){ SimpleXMLParser parser; SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); -// SemanticRoom observation = SemanticRoomXMLParser::loadRoomFromXML(path,false); -// Eigen::Matrix4f roomTransform = observation.getRoomTransform(); -// printf("roomTransform\n"); -// std::cout << roomTransform << std::endl; + // SemanticRoom observation = SemanticRoomXMLParser::loadRoomFromXML(path,false); + // Eigen::Matrix4f roomTransform = observation.getRoomTransform(); + // printf("roomTransform\n"); + // std::cout << roomTransform << std::endl; DynamicObjectXMLParser objectparser(sweep_folder, true); @@ -921,8 +1017,8 @@ int processMetaroom(std::string path, bool store_old_xml = true){ pcl::io::savePCDFileBinaryCompressed(std::string(buf),*cloud_cluster); -// std::string objectpcd = std::string(buf);//object.substr(0,object.size()-4); -// std::cout << objectpcd.substr(0,objectpcd.size()-4) << std::endl; + // std::string objectpcd = std::string(buf);//object.substr(0,object.size()-4); + // std::cout << objectpcd.substr(0,objectpcd.size()-4) << std::endl; sprintf(buf,"%s/dynamic_obj%10.10i.xml",sweep_folder.c_str(),dynamicCounter-1); QFile file(buf); @@ -934,6 +1030,7 @@ int processMetaroom(std::string path, bool store_old_xml = true){ xmlWriter->writeStartDocument(); xmlWriter->writeStartElement("Object"); xmlWriter->writeAttribute("object_number", QString::number(dynamicCounter-1)); + xmlWriter->writeAttribute("label", QString("")); xmlWriter->writeStartElement("Mean"); xmlWriter->writeAttribute("x", QString::number(sumx/sum)); xmlWriter->writeAttribute("y", QString::number(sumy/sum)); @@ -1040,6 +1137,7 @@ int processMetaroom(std::string path, bool store_old_xml = true){ xmlWriter->writeStartDocument(); xmlWriter->writeStartElement("Object"); xmlWriter->writeAttribute("object_number", QString::number(movingCounter-1)); + xmlWriter->writeAttribute("label", QString("")); xmlWriter->writeStartElement("Mean"); xmlWriter->writeAttribute("x", QString::number(sumx/sum)); xmlWriter->writeAttribute("y", QString::number(sumy/sum)); @@ -1113,7 +1211,7 @@ void trainMetaroom(std::string path){ bgmassreg->stopval = 0.0005; bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); -exit(0); + exit(0); savePoses(overall_folder+"/"+posepath,bgmfr.poses,17); fullmodel->fullDelete(); delete fullmodel; @@ -1500,13 +1598,13 @@ int main(int argc, char** argv){ roomObservationSubs.push_back(n.subscribe("/local_metric_map/room_observations", 1000, roomObservationCallback)); } -// if(m_DynamicObjectsServiceServers.size() == 0){ -// m_DynamicObjectsServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback)); -// } + // if(m_DynamicObjectsServiceServers.size() == 0){ + // m_DynamicObjectsServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback)); + // } -// if(m_GetDynamicObjectServiceServers.size() == 0){ -// m_GetDynamicObjectServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/GetDynamicObjectService", getDynamicObjectServiceCallback)); -// } + // if(m_GetDynamicObjectServiceServers.size() == 0){ + // m_GetDynamicObjectServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/GetDynamicObjectService", getDynamicObjectServiceCallback)); + // } } printf("overall_folder: %s\n",overall_folder.c_str()); diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 30b72a65..77159046 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -3,7 +3,7 @@ project(quasimodo_models) #find_package(catkin REQUIRED)# quasimodo_msgs)# COMPONENTS quasimodo_msgs) #set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") -set(CMAKE_CXX_FLAGS "-O2 -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") +set(CMAKE_CXX_FLAGS "-O2 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fopenmp -fPIC -std=c++0x -o popcnt -mssse3") find_package(catkin REQUIRED COMPONENTS #metaroom_xml_parser diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 2fb57440..c75d3567 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -67,7 +67,7 @@ namespace reglib std::vector getSuperPoints(Eigen::Matrix4d cp = Eigen::Matrix4d::Identity(), unsigned int step = 1, bool zeroinclude = true); - std::vector< std::vector > getImageProbs(); + std::vector< std::vector > getImageProbs(bool depthonly = false); }; } diff --git a/quasimodo/quasimodo_models/include/core/ReprojectionResult.h b/quasimodo/quasimodo_models/include/core/ReprojectionResult.h index ac799218..95dfd91a 100644 --- a/quasimodo/quasimodo_models/include/core/ReprojectionResult.h +++ b/quasimodo/quasimodo_models/include/core/ReprojectionResult.h @@ -12,6 +12,7 @@ class ReprojectionResult{ unsigned long dst_ind; double angle; double residualZ; + double residualE2; double residualR; double residualG; double residualB; @@ -20,7 +21,7 @@ class ReprojectionResult{ double noiseRGB; ReprojectionResult(); - ReprojectionResult(unsigned long si, unsigned long di, double rz, double a, double rr, double rg, double rb, double nz, double nrgb); + ReprojectionResult(unsigned long si, unsigned long di, double rz, double rE2, double a, double rr, double rg, double rb, double nz, double nrgb); ~ReprojectionResult(); void print(); }; diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index 7d052797..4a4e16cc 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -71,11 +71,29 @@ float recursive_split(std::vector * graphs_out,std::vector partition_graph(std::vector< std::vector< float > > & scores); inline double getNoise(double depth){return depth*depth;} + inline double getInformation(double depth){ double n = getNoise(depth); return 1.0/(n*n); } +inline double getNoise(double n1, double n2){ + double info1 = 1.0/(n1*n1); + double info2 = 1.0/(n2*n2); + double info3 = info1+info2; + return sqrt(1.0/info3); +} + +inline double getNoiseFromInformation(double info1, double info2){ + double info3 = info1+info2; + return sqrt(1.0/info3); +} + +inline double getNoiseFromInformation(double info){ + double cov = 1/info; + return sqrt(cov); +} + pcl::PointCloud::Ptr getPointCloudFromVector(std::vector & spvec, int colortype = 0, int r = -1, int g = -1, int b = -1); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 9e20e0d0..96597ce6 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -47,37 +47,37 @@ namespace reglib { unsigned long RGBDFrame_id_counter; RGBDFrame::RGBDFrame(){ - id = RGBDFrame_id_counter++; - capturetime = 0; - pose = Eigen::Matrix4d::Identity(); - keyval = ""; + id = RGBDFrame_id_counter++; + capturetime = 0; + pose = Eigen::Matrix4d::Identity(); + keyval = ""; } bool updated = true; void on_trackbar( int, void* ){updated = true;} float pred(float targetW, float targetH, int sourceW, int sourceH, int width, float * est, float * dx, float * dy){ - int ind = sourceH * width + sourceW; - return est[ind]+(targetW-float(sourceW))*dx[ind]+(targetH-float(sourceH))*dy[ind]; + int ind = sourceH * width + sourceW; + return est[ind]+(targetW-float(sourceW))*dx[ind]+(targetH-float(sourceH))*dy[ind]; } float weightFunc(int w0, int h0, int w1, int h1, int width, std::vector & est, std::vector & dx, std::vector & dy){ - float weight = 1; - for(int c = 0; c < est.size(); c++){ - float error = ((float*)(est[c].data))[h0*width+w0] - pred(w0,h0,w1,h1,width, (float*)(est[c].data), (float*)(dx[c].data), (float*)(dy[c].data)); - weight *= exp(-0.5*error*error/(0.1*0.1)); - } - return weight; + float weight = 1; + for(int c = 0; c < est.size(); c++){ + float error = ((float*)(est[c].data))[h0*width+w0] - pred(w0,h0,w1,h1,width, (float*)(est[c].data), (float*)(dx[c].data), (float*)(dy[c].data)); + weight *= exp(-0.5*error*error/(0.1*0.1)); + } + return weight; } float weightFunc(int w0, int h0, int w1, int h1, int width, std::vector & est, std::vector & dx, std::vector & dy){ - float weight = 1; - for(int c = 0; c < est.size(); c++){ - float error = est[c][h0*width+w0] - pred(w0,h0,w1,h1,width, est[c], dx[c], dy[c]); - weight *= exp(-0.5*error*error/(0.1*0.1)); - } - return weight; + float weight = 1; + for(int c = 0; c < est.size(); c++){ + float error = est[c][h0*width+w0] - pred(w0,h0,w1,h1,width, est[c], dx[c], dy[c]); + weight *= exp(-0.5*error*error/(0.1*0.1)); + } + return weight; } std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ @@ -115,16 +115,16 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} stdval = sqrt(stdval/double(Xvec.size())); - DistanceWeightFunction2PPR3 * func = new DistanceWeightFunction2PPR3(); + DistanceWeightFunction2PPR3 * func = new DistanceWeightFunction2PPR3(); func->zeromean = true; func->maxp = 0.99; func->startreg = 0.0; - func->debugg_print = false; + func->debugg_print = false; func->maxd = 256.0; func->histogram_size = 256; - //func->fixed_histogram_size = true; - func->fixed_histogram_size = false; - func->update_size = true; + //func->fixed_histogram_size = true; + func->fixed_histogram_size = false; + func->update_size = true; func->startmaxd = func->maxd; func->starthistogram_size = func->histogram_size; func->blurval = 0.005; @@ -622,39 +622,40 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt double completeStartTime = getTime(); keyval = ""; - sweepid = -1; - id = RGBDFrame_id_counter++; - camera = camera_; + sweepid = -1; + id = RGBDFrame_id_counter++; + camera = camera_; rgb = rgb_; depth = depth_; - capturetime = capturetime_; - pose = pose_; + capturetime = capturetime_; + pose = pose_; - IplImage iplimg = rgb_; - IplImage* img = &iplimg; + IplImage iplimg = rgb_; + IplImage* img = &iplimg; - int width = img->width; - int height = img->height; - const double idepth = camera->idepth_scale; - const double cx = camera->cx; - const double cy = camera->cy; - const double ifx = 1.0/camera->fx; - const double ify = 1.0/camera->fy; + unsigned int width = img->width; + unsigned int height = img->height; + unsigned int nr_pixels = width*height; + const double idepth = camera->idepth_scale; + const double cx = camera->cx; + const double cy = camera->cy; + const double ifx = 1.0/camera->fx; + const double ify = 1.0/camera->fy; - connections.resize(1); - connections[0].resize(1); - connections[0][0] = 0; + connections.resize(1); + connections[0].resize(1); + connections[0][0] = 0; - intersections.resize(1); - intersections[0].resize(1); - intersections[0][0] = 0; + intersections.resize(1); + intersections[0].resize(1); + intersections[0][0] = 0; - nr_labels = 1; - labels = new int[width*height]; - for(int i = 0; i < width*height; i++){labels[i] = 0;} + nr_labels = 1; + labels = new int[nr_pixels]; + for(int i = 0; i < nr_pixels; i++){labels[i] = 0;} - unsigned short * depthdata = (unsigned short *)depth.data; + unsigned short * depthdata = (unsigned short *)depth.data; unsigned char * rgbdata = (unsigned char *)rgb.data; if(verbose) @@ -662,40 +663,44 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt de.create(height,width,CV_32FC3); float * dedata = (float*)de.data; - for(int i = 0; i < 3*width*height; i++){dedata[i] = 0;} + for(int i = 0; i < 3*nr_pixels; i++){dedata[i] = 0;} std::vector Xvec; - int step = 1; - for(unsigned int w = step; w < width-step;w++){ - for(unsigned int h = step; h < height-step;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - - if(w > 1){ - float z2 = 0.5*idepth*float(depthdata[ind-step]+depthdata[ind+step]); - double noise1 = z*z; - double noise2 = z2*z2; - double noise3 = sqrt(noise1*noise1+noise2*noise2); - if(z2 > 0 || z > 0){ - dedata[3*ind+1] = fabs((z-z2)/noise3); - Xvec.push_back(dedata[3*ind+1]); - } - } - - if(h > 1){ - float z2 = 0.5*idepth*float(depthdata[ind-step*width]+depthdata[ind+step*width]); - double noise1 = z*z; - double noise2 = z2*z2; - double noise3 = sqrt(noise1*noise1+noise2*noise2); - if(z2 > 0 || z > 0){ - dedata[3*ind+2] = fabs((z-z2)/noise3); - Xvec.push_back(dedata[3*ind+2]); - } - } - } - } + int step = 1; + for(unsigned int w = step; w < width-step;w++){ + for(unsigned int h = step; h < height-step;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + float z2 = 0.5*idepth*float(depthdata[ind-step]+depthdata[ind+step]); + double noise1 = z*z; + double noise2 = z2*z2; + double noise3 = sqrt(noise1*noise1+noise2*noise2); + if(z2 > 0 || z > 0){ + //131 , 376 :: 240771 -> 240770 + if(ind == 240771){ + printf("ind: %i edge: %f before: %f current: %f after: %f",ind,fabs((z-z2)/noise3),idepth*float(depthdata[ind-step]),idepth*float(depthdata[ind]),idepth*float(depthdata[ind+step])); + } + dedata[3*ind+1] = fabs((z-z2)/noise3); + Xvec.push_back(dedata[3*ind+1]); + } + } + + if(h > 1){ + float z2 = 0.5*idepth*float(depthdata[ind-step*width]+depthdata[ind+step*width]); + double noise1 = z*z; + double noise2 = z2*z2; + double noise3 = sqrt(noise1*noise1+noise2*noise2); + if(z2 > 0 || z > 0){ + dedata[3*ind+2] = fabs((z-z2)/noise3); + Xvec.push_back(dedata[3*ind+2]); + } + } + } + } if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); @@ -707,9 +712,9 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} stdval = sqrt(stdval/double(Xvec.size())); - DistanceWeightFunction2PPR3 * funcZ = new DistanceWeightFunction2PPR3(); + DistanceWeightFunction2PPR3 * funcZ = new DistanceWeightFunction2PPR3(); funcZ->zeromean = true; - funcZ->startreg = 0.001; + funcZ->startreg = 0.001; funcZ->debugg_print = false; funcZ->bidir = false; funcZ->maxp = 0.999999; @@ -731,8 +736,74 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt if(h > 1){dedata[3*ind+2] = 1-funcZ->getProb(dedata[3*ind+2]);} } } + delete funcZ; + //////////////////////// + std::vector zsrc_dxdata; + zsrc_dxdata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){zsrc_dxdata[i] = 0;} + + std::vector zsrc_dydata; + zsrc_dydata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){zsrc_dydata[i] = 0;} + + std::vector zmaxima_dxdata; + zmaxima_dxdata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){zmaxima_dxdata[i] = 0;} + + std::vector zmaxima_dydata; + zmaxima_dydata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){zmaxima_dydata[i] = 0;} + + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 1){ + float z2 = idepth*float(depthdata[ind-1]); + zsrc_dxdata[ind] += fabs(z-z2); + } + + if(h > 1){ + float z2 = idepth*float(depthdata[ind-width]); + zsrc_dydata[ind] += fabs(z-z2); + } + } + } + + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + zmaxima_dxdata[ind] = (zsrc_dxdata[ind] > zsrc_dxdata[ind-1] && zsrc_dxdata[ind] >= zsrc_dxdata[ind+1]); + zmaxima_dydata[ind] = (zsrc_dydata[ind] > zsrc_dydata[ind-width] && zsrc_dydata[ind] >= zsrc_dydata[ind+width]); + } + } + +// cv::Mat zmax; +// zmax.create(height,width,CV_32FC3); +// float * zmaxdata = (float*)zmax.data; +// for(int i = 0; i < nr_pixels; i++){ +// zmaxdata[3*i+0] = 0; +// zmaxdata[3*i+1] = dedata[3*i+1]*zmaxima_dxdata[i]; +// zmaxdata[3*i+2] = dedata[3*i+2]*zmaxima_dydata[i]; +// } + +// // cv::namedWindow( "colour", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour", ce); +// // cv::namedWindow( "colour+nms", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour+nms", cenms); +// cv::namedWindow( "zmax", cv::WINDOW_AUTOSIZE ); cv::imshow( "zmax", zmax); +// cv::waitKey(0); + + for(int i = 0; i < nr_pixels; i++){ +// dedata[3*i+1] = std::max(dedata[3*i+1]*zmaxima_dxdata[i],0.0001f); +// dedata[3*i+2] = std::max(dedata[3*i+2]*zmaxima_dydata[i],0.0001f); + } + + //////////////////////// + if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); @@ -744,21 +815,21 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt cv::Mat re; re.create(height,width,CV_32FC3); float * redata = (float*)re.data; - for(int i = 0; i < 3*width*height; i++){redata[i] = 0;} + for(int i = 0; i < 3*nr_pixels; i++){redata[i] = 0;} cv::Mat ge; ge.create(height,width,CV_32FC3); float * gedata = (float*)ge.data; - for(int i = 0; i < 3*width*height; i++){gedata[i] = 0;} + for(int i = 0; i < 3*nr_pixels; i++){gedata[i] = 0;} cv::Mat be; be.create(height,width,CV_32FC3); float * bedata = (float*)be.data; - for(int i = 0; i < 3*width*height; i++){bedata[i] = 0;} + for(int i = 0; i < 3*nr_pixels; i++){bedata[i] = 0;} ce.create(height,width,CV_32FC3); float * cedata = (float*)ce.data; - for(int i = 0; i < 3*width*height; i++){cedata[i] = 0;} + for(int i = 0; i < 3*nr_pixels; i++){cedata[i] = 0;} std::vector XvecR; std::vector XvecG; @@ -791,20 +862,20 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } std::vector src_dxdata; - src_dxdata.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){src_dxdata[i] = 0;} + src_dxdata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){src_dxdata[i] = 0;} std::vector src_dydata; - src_dydata.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){src_dydata[i] = 0;} + src_dydata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){src_dydata[i] = 0;} std::vector maxima_dxdata; - maxima_dxdata.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){maxima_dxdata[i] = 0;} + maxima_dxdata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){maxima_dxdata[i] = 0;} std::vector maxima_dydata; - maxima_dydata.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){maxima_dydata[i] = 0;} + maxima_dydata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){maxima_dydata[i] = 0;} for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ @@ -828,8 +899,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt for(unsigned int w = 1; w < width-1;w++){ for(unsigned int h = 1; h < height-1;h++){ int ind = h*width+w; - maxima_dxdata[ind] = (src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] >= src_dxdata[ind+1]); - maxima_dydata[ind] = (src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] >= src_dydata[ind+width]); + maxima_dxdata[ind] = (src_dxdata[ind] > src_dxdata[ind-1] && src_dxdata[ind] >= src_dxdata[ind+1]); + maxima_dydata[ind] = (src_dydata[ind] > src_dydata[ind-width] && src_dydata[ind] >= src_dydata[ind+width]); } } @@ -853,7 +924,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt DistanceWeightFunction2PPR2 * funcB = new DistanceWeightFunction2PPR2(); funcR->zeromean = funcG->zeromean = funcB->zeromean = true; funcR->maxp = funcG->maxp = funcB->maxp = 0.9999; - funcR->startreg = funcG->startreg = funcB->startreg = 0.5; + funcR->startreg = funcG->startreg = funcB->startreg = 1.0; funcR->debugg_print = funcG->debugg_print = funcB->debugg_print = false; funcR->maxd = funcG->maxd = funcB->maxd = 256.0; funcR->histogram_size = funcG->histogram_size = funcB->histogram_size = 256; @@ -912,192 +983,134 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); - cv::Mat cenms; - cenms.create(height,width,CV_32FC3); - float * cenmsdata = (float*)cenms.data; - for(int i = 0; i < width*height; i++){ - cenmsdata[3*i+0] = 0; - cenmsdata[3*i+1] = cedata[3*i+1]*maxima_dxdata[i]; - cenmsdata[3*i+2] = cedata[3*i+2]*maxima_dydata[i]; - } - cv::Mat det; det.create(height,width,CV_8UC1); unsigned char * detdata = (unsigned char*)det.data; - for(int i = 0; i < width*height; i++){ + for(int i = 0; i < nr_pixels; i++){ detdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5)); } -// cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", de); -// cv::namedWindow( "re", cv::WINDOW_AUTOSIZE ); cv::imshow( "re", re); -// cv::namedWindow( "ge", cv::WINDOW_AUTOSIZE ); cv::imshow( "ge", ge); -// cv::namedWindow( "be", cv::WINDOW_AUTOSIZE ); cv::imshow( "be", be); -// cv::namedWindow( "colour", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour", ce); -// cv::namedWindow( "colour+nms", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour+nms", cenms); -// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); -// cv::namedWindow( "blur", cv::WINDOW_AUTOSIZE ); cv::imshow( "blur", blur_rgb); -// //cv::namedWindow( "maximas", cv::WINDOW_AUTOSIZE ); cv::imshow( "blur", blur_rgb); -// cv::waitKey(0); - // cv::namedWindow( "det", cv::WINDOW_AUTOSIZE ); cv::imshow( "det", det); - // cv::waitKey(0); - - - int dilation_size = 4; - cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) ); + cv::Mat cenms; + cenms.create(height,width,CV_32FC3); + float * cenmsdata = (float*)cenms.data; + for(int i = 0; i < nr_pixels; i++){ + double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]); + edgep = std::max(0.0001,edgep); + cenmsdata[3*i+0] = edgep; + cenmsdata[3*i+1] = edgep; + cenmsdata[3*i+2] = edgep; + } + +// for(int i = 0; i < nr_pixels; i++){ +// double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]); +// edgep = std::max(0.00001,edgep); +// cenmsdata[3*i+0] = 0; +// cenmsdata[3*i+1] = std::max(0.00001,cenmsdata[3*i+1]); +// cenmsdata[3*i+2] = std::max(0.00001,edgep); +// } + + // cv::namedWindow( "colour", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour", ce); + // cv::namedWindow( "colour+nms", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour+nms", cenms); + // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); + // cv::waitKey(0); + ce = cenms; + int dilation_size = 4; + cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) ); unsigned char * det_dilatedata = (unsigned char*)det_dilate.data; - depthedges.create(height,width,CV_8UC1); - unsigned char * depthedgesdata = (unsigned char *)depthedges.data; - for(int i = 0; i < width*height; i++){ + depthedges.create(height,width,CV_8UC1); + unsigned char * depthedgesdata = (unsigned char *)depthedges.data; + for(int i = 0; i < nr_pixels; i++){ //depthedgesdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5) || (cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5)); depthedgesdata[i] = 255*(((cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5)) && (det_dilatedata[i] == 0)); - //depthedgesdata[i] = 0; } - - //cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges); - //cv::waitKey(0); + // cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges); + // cv::waitKey(0); if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); - if(compute_normals){ - normals.create(height,width,CV_32FC3); - float * normalsdata = (float *)normals.data; - - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr normals_cloud (new pcl::PointCloud); - cloud->width = width; - cloud->height = height; - cloud->points.resize(width*height); - - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBA & p = cloud->points[ind]; - p.b = rgbdata[3*ind+0]; - p.g = rgbdata[3*ind+1]; - p.r = rgbdata[3*ind+2]; - double z = idepth*double(depthdata[ind]); - if(z > 0){ - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - }else{ - p.x = NAN; - p.y = NAN; - p.z = NAN; - } - } - } + if(compute_normals){ + normals.create(height,width,CV_32FC3); + float * normalsdata = (float *)normals.data; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr normals_cloud (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(nr_pixels); + + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::PointXYZRGBA & p = cloud->points[ind]; + p.b = rgbdata[3*ind+0]; + p.g = rgbdata[3*ind+1]; + p.r = rgbdata[3*ind+2]; + double z = idepth*double(depthdata[ind]); + if(z > 0){ + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + }else{ + p.x = NAN; + p.y = NAN; + p.z = NAN; + } + } + } pcl::IntegralImageNormalEstimation ne; - ne.setInputCloud(cloud); - - int MaxDepthChangeFactor = 20; - int NormalSmoothingSize = 7; - int depth_dependent_smoothing = 1; - - ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); - ne.setNormalSmoothingSize(NormalSmoothingSize); - ne.setDepthDependentSmoothing (depth_dependent_smoothing); - ne.compute(*normals_cloud); - - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBA p = cloud->points[ind]; - pcl::Normal p2 = normals_cloud->points[ind]; - if(!isnan(p2.normal_x)){ - normalsdata[3*ind+0] = p2.normal_x; - normalsdata[3*ind+1] = p2.normal_y; - normalsdata[3*ind+2] = p2.normal_z; - }else{ - normalsdata[3*ind+0] = 2; - normalsdata[3*ind+1] = 2; - normalsdata[3*ind+2] = 2; - } - } - } -if(false){ - //pcl::PointCloud::Ptr cloud2 = getPCLcloud(); - pcl::OrganizedEdgeFromRGBNormals oed; - oed.setInputNormals (normals_cloud); - oed.setInputCloud (cloud); - oed.setDepthDisconThreshold (0.02); // 2cm - // oed.setHCCannyLowThreshold (0.4); - // oed.setHCCannyHighThreshold (1.1); - oed.setMaxSearchNeighbors (50); - pcl::PointCloud labels; - std::vector label_indices; - oed.compute (labels, label_indices); - - // pcl::PointCloud::Ptr occluding_edges (new pcl::PointCloud), - // occluded_edges (new pcl::PointCloud), - // boundary_edges (new pcl::PointCloud), - // high_curvature_edges (new pcl::PointCloud), - // rgb_edges (new pcl::PointCloud); - - // pcl::PointCloud::Ptr cloud3 (new pcl::PointCloud); - // pcl::copyPointCloud (*cloud2, label_indices[0].indices, *boundary_edges); - // pcl::copyPointCloud (*cloud2, label_indices[1].indices, *occluding_edges); - // pcl::copyPointCloud (*cloud2, label_indices[2].indices, *occluded_edges); - // pcl::copyPointCloud (*cloud2, label_indices[3].indices, *high_curvature_edges); - // pcl::copyPointCloud (*cloud2, label_indices[4].indices, *rgb_edges); - - // cv::Mat out = rgb.clone(); - - for(unsigned int i = 0; i < label_indices[1].indices.size(); i++){ - int ind = label_indices[1].indices[i]; - //depthedgesdata[ind] = 1; - // out.data[3*ind+0] =255; - // out.data[3*ind+1] =0; - // out.data[3*ind+2] =255; - } - - // for(unsigned int i = 0; i < label_indices[3].indices.size(); i++){ - // int ind = label_indices[3].indices[i]; - // depthedgesdata[ind] = 255; - // out.data[3*ind+0] =0; - // out.data[3*ind+1] =255; - // out.data[3*ind+2] =255; - // } - - - for(unsigned int i = 0; i < label_indices[4].indices.size(); i++){ - int ind = label_indices[4].indices[i]; - depthedgesdata[ind] = 255; - // out.data[3*ind+0] =0; - // out.data[3*ind+1] =255; - // out.data[3*ind+2] =0; - } - } - // cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); - // cv::imshow( "edges", out ); - // cv::waitKey(0); + ne.setInputCloud(cloud); - //show(true); - } + int MaxDepthChangeFactor = 20; + int NormalSmoothingSize = 7; + int depth_dependent_smoothing = 1; + + ne.setMaxDepthChangeFactor(0.001*double(MaxDepthChangeFactor)); + ne.setNormalSmoothingSize(NormalSmoothingSize); + ne.setDepthDependentSmoothing (depth_dependent_smoothing); + ne.compute(*normals_cloud); + + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::Normal p2 = normals_cloud->points[ind]; + if(!isnan(p2.normal_x)){ + normalsdata[3*ind+0] = p2.normal_x; + normalsdata[3*ind+1] = p2.normal_y; + normalsdata[3*ind+2] = p2.normal_z; + }else{ + normalsdata[3*ind+0] = 2; + normalsdata[3*ind+1] = 2; + normalsdata[3*ind+2] = 2; + } + } + } + } if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); printf("complete time to create RGBD image: %5.5fs\n",getTime()-completeStartTime); + + //getImageProbs(); } RGBDFrame::~RGBDFrame(){ - rgb.release(); - normals.release(); - depth.release(); - depthedges.release(); + rgb.release(); + normals.release(); + depth.release(); + depthedges.release(); //if(rgbdata != 0){delete[] rgbdata; rgbdata = 0;} - if(labels != 0){delete[] labels; labels = 0;} + if(labels != 0){delete[] labels; labels = 0;} } void RGBDFrame::show(bool stop){ - cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); - cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); cv::imshow( "normals", normals ); - cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", depth ); - cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges ); - if(stop){ cv::waitKey(0);}else{ cv::waitKey(30);} + cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); + cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); cv::imshow( "normals", normals ); + cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", depth ); + cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges ); + if(stop){ cv::waitKey(0);}else{ cv::waitKey(30);} } pcl::PointCloud::Ptr RGBDFrame::getSmallPCLcloud(){ @@ -1134,175 +1147,184 @@ pcl::PointCloud::Ptr RGBDFrame::getSmallPCLcloud(){ } pcl::PointCloud::Ptr RGBDFrame::getPCLcloud(){ - unsigned char * rgbdata = (unsigned char *)rgb.data; - unsigned short * depthdata = (unsigned short *)depth.data; - - const unsigned int width = camera->width; const unsigned int height = camera->height; - const double idepth = camera->idepth_scale; - const double cx = camera->cx; const double cy = camera->cy; - const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy; - - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr normals (new pcl::PointCloud); - cloud->width = width; - cloud->height = height; - cloud->points.resize(width*height); - - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - double z = idepth*double(depthdata[ind]); - - pcl::PointXYZRGB p; - p.b = rgbdata[3*ind+0]; - p.g = rgbdata[3*ind+1]; - p.r = rgbdata[3*ind+2]; - if(z > 0){ - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - }else{ - p.x = NAN; - p.y = NAN; - p.z = NAN; - } - cloud->points[ind] = p; - } - } - - pcl::IntegralImageNormalEstimation ne; - ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); - ne.setMaxDepthChangeFactor(0.02f); - ne.setNormalSmoothingSize(10.0f); - ne.setInputCloud(cloud); - ne.compute(*normals); - - pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); - cloud_ptr->width = width; - cloud_ptr->height = height; - cloud_ptr->points.resize(width*height); - - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBNormal & p0 = cloud_ptr->points[ind]; - pcl::PointXYZRGB p1 = cloud->points[ind]; - pcl::Normal p2 = normals->points[ind]; - p0.x = p1.x; - p0.y = p1.y; - p0.z = p1.z; - p0.r = p1.r; - p0.g = p1.g; - p0.b = p1.b; - p0.normal_x = p2.normal_x; - p0.normal_y = p2.normal_y; - p0.normal_z = p2.normal_z; - } - } - return cloud_ptr; + unsigned char * rgbdata = (unsigned char *)rgb.data; + unsigned short * depthdata = (unsigned short *)depth.data; + + const unsigned int width = camera->width; const unsigned int height = camera->height; + const double idepth = camera->idepth_scale; + const double cx = camera->cx; const double cy = camera->cy; + const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr normals (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(width*height); + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + double z = idepth*double(depthdata[ind]); + + pcl::PointXYZRGB p; + p.b = rgbdata[3*ind+0]; + p.g = rgbdata[3*ind+1]; + p.r = rgbdata[3*ind+2]; + if(z > 0){ + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + }else{ + p.x = NAN; + p.y = NAN; + p.z = NAN; + } + cloud->points[ind] = p; + } + } + + pcl::IntegralImageNormalEstimation ne; + ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); + ne.setMaxDepthChangeFactor(0.02f); + ne.setNormalSmoothingSize(10.0f); + ne.setInputCloud(cloud); + ne.compute(*normals); + + pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); + cloud_ptr->width = width; + cloud_ptr->height = height; + cloud_ptr->points.resize(width*height); + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::PointXYZRGBNormal & p0 = cloud_ptr->points[ind]; + pcl::PointXYZRGB p1 = cloud->points[ind]; + pcl::Normal p2 = normals->points[ind]; + + p0.x = p1.x; + p0.y = p1.y; + p0.z = p1.z; + + if(depthdata[ind] == 0){ + p0.x = 0; + p0.y = 0; + p0.z = 0; + } + + p0.r = p1.r; + p0.g = p1.g; + p0.b = p1.b; + p0.normal_x = p2.normal_x; + p0.normal_y = p2.normal_y; + p0.normal_z = p2.normal_z; + } + } + + return cloud_ptr; } void RGBDFrame::savePCD(std::string path, Eigen::Matrix4d pose){ - // printf("saving pcd: %s\n",path.c_str()); - unsigned char * rgbdata = (unsigned char *)rgb.data; - unsigned short * depthdata = (unsigned short *)depth.data; - - const unsigned int width = camera->width; const unsigned int height = camera->height; - const double idepth = camera->idepth_scale; - const double cx = camera->cx; const double cy = camera->cy; - const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy; - - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - cloud->width = width; - cloud->height = height; - cloud->points.resize(width*height); - - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - double z = idepth*double(depthdata[ind]); - if(z > 0){ - pcl::PointXYZRGB p; - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - p.b = rgbdata[3*ind+0]; - p.g = rgbdata[3*ind+1]; - p.r = rgbdata[3*ind+2]; - cloud->points[ind] = p; - } - } - } - - //Mat4f2RotTrans(const Eigen::Matrix4f &tf, Eigen::Quaternionf &q, Eigen::Vector4f &trans) - Mat4f2RotTrans(pose.cast(),cloud->sensor_orientation_,cloud->sensor_origin_); - int success = pcl::io::savePCDFileBinaryCompressed(path,*cloud); + // printf("saving pcd: %s\n",path.c_str()); + unsigned char * rgbdata = (unsigned char *)rgb.data; + unsigned short * depthdata = (unsigned short *)depth.data; + + const unsigned int width = camera->width; const unsigned int height = camera->height; + const double idepth = camera->idepth_scale; + const double cx = camera->cx; const double cy = camera->cy; + const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(width*height); + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + double z = idepth*double(depthdata[ind]); + if(z > 0){ + pcl::PointXYZRGB p; + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + p.b = rgbdata[3*ind+0]; + p.g = rgbdata[3*ind+1]; + p.r = rgbdata[3*ind+2]; + cloud->points[ind] = p; + } + } + } + + //Mat4f2RotTrans(const Eigen::Matrix4f &tf, Eigen::Quaternionf &q, Eigen::Vector4f &trans) + Mat4f2RotTrans(pose.cast(),cloud->sensor_orientation_,cloud->sensor_origin_); + int success = pcl::io::savePCDFileBinaryCompressed(path,*cloud); } void RGBDFrame::save(std::string path){ - //printf("saving frame %i to %s\n",id,path.c_str()); - - cv::imwrite( path+"_rgb.png", rgb ); - cv::imwrite( path+"_depth.png", depth ); - - unsigned long buffersize = 19*sizeof(double); - char* buffer = new char[buffersize]; - double * buffer_double = (double *)buffer; - unsigned long * buffer_long = (unsigned long *)buffer; - - int counter = 0; - buffer_double[counter++] = capturetime; - for(int i = 0; i < 4; i++){ - for(int j = 0; j < 4; j++){ - buffer_double[counter++] = pose(i,j); - } - } - buffer_long[counter++] = sweepid; - buffer_long[counter++] = camera->id; - std::ofstream outfile (path+"_data.txt",std::ofstream::binary); - outfile.write (buffer,buffersize); - outfile.close(); - delete[] buffer; + //printf("saving frame %i to %s\n",id,path.c_str()); + + cv::imwrite( path+"_rgb.png", rgb ); + cv::imwrite( path+"_depth.png", depth ); + + unsigned long buffersize = 19*sizeof(double); + char* buffer = new char[buffersize]; + double * buffer_double = (double *)buffer; + unsigned long * buffer_long = (unsigned long *)buffer; + + int counter = 0; + buffer_double[counter++] = capturetime; + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + buffer_double[counter++] = pose(i,j); + } + } + buffer_long[counter++] = sweepid; + buffer_long[counter++] = camera->id; + std::ofstream outfile (path+"_data.txt",std::ofstream::binary); + outfile.write (buffer,buffersize); + outfile.close(); + delete[] buffer; } RGBDFrame * RGBDFrame::load(Camera * cam, std::string path){ //printf("RGBDFrame * RGBDFrame::load(Camera * cam, std::string path)\n"); - std::streampos size; - char * buffer; - char buf [1024]; - std::string datapath = path+"_data.txt"; - std::ifstream file (datapath, std::ios::in | std::ios::binary | std::ios::ate); - if (file.is_open()){ - size = file.tellg(); - buffer = new char [size]; - file.seekg (0, std::ios::beg); - file.read (buffer, size); - file.close(); - - double * buffer_double = (double *)buffer; - unsigned long * buffer_long = (unsigned long *)buffer; - - int counter = 0; - double capturetime = buffer_double[counter++]; - Eigen::Matrix4d pose; - for(int i = 0; i < 4; i++){ - for(int j = 0; j < 4; j++){ - pose(i,j) = buffer_double[counter++]; - } - } - int sweepid = buffer_long[counter++]; - int camera_id = buffer_long[counter++]; - - cv::Mat rgb = cv::imread(path+"_rgb.png", -1); // Read the file - cv::Mat depth = cv::imread(path+"_depth.png", -1); // Read the file - - RGBDFrame * frame = new RGBDFrame(cam,rgb,depth,capturetime,pose); - frame->sweepid = sweepid; - //printf("sweepid: %i",sweepid); - - return frame; - }else{printf("cant open %s\n",(path+"/data.txt").c_str());} + std::streampos size; + char * buffer; + char buf [1024]; + std::string datapath = path+"_data.txt"; + std::ifstream file (datapath, std::ios::in | std::ios::binary | std::ios::ate); + if (file.is_open()){ + size = file.tellg(); + buffer = new char [size]; + file.seekg (0, std::ios::beg); + file.read (buffer, size); + file.close(); + + double * buffer_double = (double *)buffer; + unsigned long * buffer_long = (unsigned long *)buffer; + + int counter = 0; + double capturetime = buffer_double[counter++]; + Eigen::Matrix4d pose; + for(int i = 0; i < 4; i++){ + for(int j = 0; j < 4; j++){ + pose(i,j) = buffer_double[counter++]; + } + } + int sweepid = buffer_long[counter++]; + int camera_id = buffer_long[counter++]; + + cv::Mat rgb = cv::imread(path+"_rgb.png", -1); // Read the file + cv::Mat depth = cv::imread(path+"_depth.png", -1); // Read the file + + RGBDFrame * frame = new RGBDFrame(cam,rgb,depth,capturetime,pose); + frame->sweepid = sweepid; + //printf("sweepid: %i",sweepid); + + return frame; + }else{printf("cant open %s\n",(path+"/data.txt").c_str());} } std::vector RGBDFrame::getReprojections(std::vector & spvec, Eigen::Matrix4d cp, bool * maskvec, bool useDet){ @@ -1370,11 +1392,12 @@ std::vector RGBDFrame::getReprojections(std::vector RGBDFrame::getSuperPoints(Eigen::Matrix4d cp, unsigned i for(unsigned long h = 0; h < height;h += step){ for(unsigned long w = 0; w < width;w += step){ unsigned int ind = h * width + w; - //superpoint & p = ret[ind]; Eigen::Vector3f rgb (rgbdata[3*ind+0],rgbdata[3*ind+1],rgbdata[3*ind+2]); float z = idepth*float(depthdata[ind]); float nx = normalsdata[3*ind+0]; @@ -1423,10 +1445,8 @@ std::vector RGBDFrame::getSuperPoints(Eigen::Matrix4d cp, unsigned i float tny = m10*nx + m11*ny + m12*nz; float tnz = m20*nx + m21*ny + m22*nz; - //ret[ind] = superpoint(Vector3f(tx,ty,tz),Vector3f(tnx,tny,tnz),rgb, getInformation(z), 1, 0); ret[count++] = superpoint(Vector3f(tx,ty,tz),Vector3f(tnx,tny,tnz),rgb, getInformation(z), 1, 0); }else{ - //ret[ind] = superpoint(Eigen::Vector3f(0,0,0),Eigen::Vector3f(0,0,0),rgb, 0, 1, 0); if(zeroinclude){ ret[count++] = superpoint(Eigen::Vector3f(0,0,0),Eigen::Vector3f(0,0,0),rgb, 0, 1, 0); } @@ -1437,22 +1457,56 @@ std::vector RGBDFrame::getSuperPoints(Eigen::Matrix4d cp, unsigned i return ret; } -std::vector< std::vector > RGBDFrame::getImageProbs(){ +std::vector< std::vector > RGBDFrame::getImageProbs(bool depthonly){ + const unsigned int width = camera->width; + const unsigned int height = camera->height; + const unsigned int nr_pixels = width*height; - unsigned int width = camera->width; - unsigned int height = camera->height; + + float * cedata = (float*)ce.data; + float * dedata = (float*)de.data; + unsigned char * det_dilatedata = det_dilate.data; std::vector dxc; - dxc.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0;} + dxc.resize(nr_pixels); std::vector dyc; - dyc.resize(width*height); - for(unsigned int i = 0; i < width*height;i++){dyc[i] = 0;} + dyc.resize(nr_pixels); + + for(unsigned int i = 0; i < nr_pixels;i++){ +// dxc[i] = dedata[3*i+1]; +// dyc[i] = dedata[3*i+2]; + + if(!det_dilatedata[i]){ + dxc[i] = cedata[3*i+1]; + dyc[i] = cedata[3*i+2]; + }else{ + dxc[i] = std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); + dyc[i] = std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); + } + + dxc[i] = std::min(dxc[i],dyc[i]); + dyc[i] = dxc[i];// std::min(probX,probY); + } + +/* + + probY = 1-current; +} + +dxc[ind] = std::min(probX,probY); +dyc[ind] = std::min(probX,probY); +*/ +// cv::namedWindow( "Colour edges" , cv::WINDOW_AUTOSIZE ); cv::imshow( "Colour edges", ce); +// cv::namedWindow( "Depth edges" , cv::WINDOW_AUTOSIZE ); cv::imshow( "Depth edges", de); +// cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); + +// cv::waitKey(0); std::vector< std::vector > probs2; probs2.push_back(dxc); probs2.push_back(dyc); + return probs2; } } diff --git a/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp b/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp index 12102698..c1d052bc 100644 --- a/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp +++ b/quasimodo/quasimodo_models/src/core/ReprojectionResult.cpp @@ -4,12 +4,13 @@ namespace reglib { ReprojectionResult::ReprojectionResult(){} -ReprojectionResult::ReprojectionResult(unsigned long si, unsigned long di, double a, double rz, double rr, double rg, double rb, double nz, double nrgb){ +ReprojectionResult::ReprojectionResult(unsigned long si, unsigned long di, double a, double rz, double rE2, double rr, double rg, double rb, double nz, double nrgb){ src_ind = si; dst_ind = di; angle = a; residualZ = rz; + residualE2 = rE2; residualR = rr; residualG = rg; residualB = rb; diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 9d740102..8bd3bd7c 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1239,9 +1239,9 @@ cv::Mat getImageFromArray(unsigned int width, unsigned int height, double * arr) } } -// cv::namedWindow( "getImageFromArray", cv::WINDOW_AUTOSIZE ); -// cv::imshow( "getImageFromArray",m); -// cv::waitKey(0); + // cv::namedWindow( "getImageFromArray", cv::WINDOW_AUTOSIZE ); + // cv::imshow( "getImageFromArray",m); + // cv::waitKey(0); return m; } @@ -1857,9 +1857,9 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int b edgesdata[3*i+2] = dyc[i]; } -// cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); -// cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); -// cv::waitKey(0); + // cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); + // cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); + // cv::waitKey(0); return probs2; } @@ -1964,6 +1964,11 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dst_cloud = getPointCloudFromVector(framesp2,3,0,0,255); } + double threshold = 0; + if(!store_distance){ + threshold = pow(20.0*dfunc->getNoise(),2); + } + int totsum = 0; for(unsigned long ind = 0; ind < nr_rr;ind++){ ReprojectionResult & rr = rr_vec[ind]; @@ -1984,13 +1989,14 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec.push_back(d); nvec.push_back(1-surface_angle); }else{ + double dE2 = rr.residualE2/total_stdiv; double p_overlap_angle = nfunc->getProb(1-surface_angle); double p_overlap = dfunc->getProb(d); double p_occlusion = dfunc->getProbInfront(d); p_overlap *= p_overlap_angle; - if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0){ + if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0 && dE2 < threshold){ interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap)); } @@ -2028,8 +2034,6 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & } } - -//SegmentationResults ModelUpdater::computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ double computeMovingDynamicStatic_startTime = getTime(); @@ -2078,7 +2082,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s printf("bg init time: %5.5fs\n",getTime()-startTime); startTime = getTime(); -// printf("computing all residuals\n"); for(unsigned int i = 0; i < frames.size(); i++){ std::vector & framesp1_test = framesp_test[i]; std::vector & framesp1 = framesp[i]; @@ -2090,8 +2093,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } - -// printf("training ppr\n"); double dstdval = 0; for(unsigned int i = 0; i < dvec.size(); i++){dstdval += dvec[i]*dvec[i];} dstdval = sqrt(dstdval/double(dvec.size()-1)); @@ -2100,8 +2101,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dfunc = dfuncTMP; dfuncTMP->refine_mean = true; dfuncTMP->zeromean = false; + dfuncTMP->startreg = 0.0; //dfuncTMP->startreg = 0.001; - dfuncTMP->startreg = 0.0000001; + //dfuncTMP->startreg = 0.0000001; dfuncTMP->debugg_print = false; dfuncTMP->bidir = true; //dfuncTMP->bidir = false; @@ -2154,13 +2156,14 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s double total_dealloctime = 0; double total_Dynw = 0; -// printf("adding constraints\n"); for(unsigned int i = 0; i < frames.size(); i++){ printf("currently workin on frame %i\n",i); int offset = offsets[i]; RGBDFrame * frame = frames[i]; startTime = getTime(); + //std::vector< std::vector > probs = frame->getImageProbs();//getImageProbs(frame,9); std::vector< std::vector > probs = getImageProbs(frame,9); + total_connectiontime += getTime()-startTime; pixel_weights.push_back(probs); unsigned short * depthdata = (unsigned short *)(frame->depth.data); @@ -2284,9 +2287,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s printf("total_dealloctime = %5.5fs\n", total_dealloctime); printf("total_Dynw = %5.5fs\n", total_Dynw); - - printf("setup inference\n"); - long interframeConnections = 0; for(unsigned int i = 0; i < interframe_connectionId.size();i++){ for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ @@ -2341,15 +2341,75 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } } + printf("run inference\n"); + + std::vector > interframe_connectionId_added; + interframe_connectionId_added.resize(interframe_connectionId.size()); + + std::vector > interframe_connectionStrength_added; + interframe_connectionStrength_added.resize(interframe_connectionStrength.size()); + + double tot_inf = 0; + for(unsigned int it = 0; it < 10; it++){ + double start_inf1 = getTime(); + + g -> maxflow(); + + double diffs = 0; + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + double weight = interframe_connectionStrength[i][j]; + unsigned long other = interframe_connectionId[i][j]; + if(weight > 1 && g->what_segment(i) != g->what_segment(other)){ + diffs++; + } + } + } + + if(diffs < 1000){break;} + + double adds = 100000; + double prob = std::min(adds / diffs,1.0); + printf("diffs: %f adds: %f prob: %f\n",diffs,adds,prob); + + double trueadds = 0; + + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + double weight = interframe_connectionStrength[i][j]; + unsigned long other = interframe_connectionId[i][j]; + if(weight > 0.1 && g->what_segment(i) != g->what_segment(other)){ + if(rand() <= prob*RAND_MAX){ + trueadds++; + g -> add_edge( i, other, weight, weight ); + + interframe_connectionId_added[i].push_back(other); + interframe_connectionStrength_added[i].push_back(weight); + + interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back(); + interframe_connectionStrength[i].pop_back(); + + interframe_connectionId[i][j] = interframe_connectionId[i].back(); + interframe_connectionId[i].pop_back(); + j--; + } + } + } + } + printf("trueadds: %f\n",trueadds); + + tot_inf += getTime()-start_inf1; + printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf); + + if(tot_inf > 90){break;} + } for(unsigned int i = 0; i < interframe_connectionId.size();i++){ - for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ - double weight = interframe_connectionStrength[i][j]; - //g -> add_edge( i, interframe_connectionId[i][j], weight, weight ); + for(unsigned int j = 0; j < interframe_connectionId_added[i].size();j++){ + interframe_connectionStrength[i].push_back(interframe_connectionStrength_added[i][j]); + interframe_connectionId[i].push_back(interframe_connectionId_added[i][j]); } } - printf("run inference\n"); - g -> maxflow(); int dynamic_label = 1; std::vector labels; @@ -2369,12 +2429,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s delete g; double end_inf = getTime(); - printf("static inference time: %10.10fs\n",end_inf-start_inf); std::vector< std::vector< unsigned long > > component_dynamic; - std::vector< double > scores_dynamic; - std::vector< double > total_dynamic; - std::vector< std::vector< unsigned long > > component_moving; - std::vector< double > scores_moving; - std::vector< double > total_moving; + printf("static inference time: %10.10fs\n",end_inf-start_inf); long dynamic_frameConnections = 0; @@ -2471,14 +2526,83 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ if(labels[interframe_connectionId[i][j]] == labels[i]){ double weight = interframe_connectionStrength[i][j]; - //dynamic_g -> add_edge( stat_ind[i], stat_ind[ interframe_connectionId[i][j] ], weight, weight ); + dynamic_g -> add_edge( stat_ind[i], stat_ind[ interframe_connectionId[i][j] ], weight, weight ); } } } } - dynamic_g -> maxflow(); + + tot_inf = 0; + for(unsigned int it = 0; false && it < 100; it++){ + double start_inf1 = getTime(); + dynamic_g -> maxflow(); + + double diffs = 0; + for(unsigned long i = 0; i < current_point;i++){ + if(labels[i] == dynamic_label){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + if(labels[interframe_connectionId[i][j]] == labels[i]){ + double weight = interframe_connectionStrength[i][j]; + + unsigned long i2 = stat_ind[i]; + unsigned long j2 = stat_ind[ interframe_connectionId[i][j] ]; + if(weight > 1 && g->what_segment(i2) != g->what_segment(j2)){ + diffs++; + } + } + } + } + } + + + double adds = 100000; + double prob = std::min(adds / diffs,1.0); + printf("diffs: %f adds: %f prob: %f\n",diffs,adds,prob); + + if(diffs < 1000){break;} + + + double trueadds = 0; + + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + if(labels[i] == dynamic_label){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + if(labels[interframe_connectionId[i][j]] == labels[i]){ + + double weight = interframe_connectionStrength[i][j]; + unsigned long other = interframe_connectionId[i][j]; + + unsigned long i2 = stat_ind[i]; + unsigned long j2 = stat_ind[ interframe_connectionId[i][j] ]; + if(weight > 1 && g->what_segment(i2) != g->what_segment(j2)){ + if(rand() <= prob*RAND_MAX){ + trueadds++; + g -> add_edge( i2, j2, weight, weight ); + + interframe_connectionId_added[i].push_back(other); + interframe_connectionStrength_added[i].push_back(weight); + + interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back(); + interframe_connectionStrength[i].pop_back(); + + interframe_connectionId[i][j] = interframe_connectionId[i].back(); + interframe_connectionId[i].pop_back(); + j--; + } + } + } + } + } + } + printf("trueadds: %f\n",trueadds); + + tot_inf += getTime()-start_inf1; + printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf); + if(tot_inf > 90){break;} + } + end_inf = getTime(); printf("dynamic inference time: %10.10fs\n",end_inf-start_inf); @@ -2492,6 +2616,13 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } delete dynamic_g; + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId_added[i].size();j++){ + interframe_connectionStrength[i].push_back(interframe_connectionStrength_added[i][j]); + interframe_connectionId[i].push_back(interframe_connectionId_added[i][j]); + } + } + const unsigned int nr_frames = frames.size(); const unsigned int width = frames[0]->camera->width; const unsigned int height = frames[0]->camera->height; @@ -2499,6 +2630,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s const unsigned int nr_pixels = nr_frames*pixels_per_image; double probthresh = 0.5; + double str_probthresh = -log(probthresh); unsigned int number_of_dynamics = 0; unsigned int nr_obj_dyn = 0; unsigned int nr_obj_mov = 0; @@ -2567,17 +2699,54 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for(unsigned long j = 0; j < interframe_connectionId[cind].size();j++){ unsigned long other = interframe_connectionId[cind][j]; - if( objectlabel[other] == 0 && labels[other] == current_label){ + if(interframe_connectionStrength[cind][j] > str_probthresh && objectlabel[other] == 0 && labels[other] == current_label){ objectlabel[other] = number_of_dynamics; todo.push_back(other); } } } + printf("score: %f\n",score); labelID.push_back(0); if(score < 200){continue;} + + if(debugg && todo.size() > 10000){ + printf("3-class\n"); + for(unsigned int i = 0; i < current_point; i++){ + cloud->points[i].r = priors[3*i+0]*255.0; + cloud->points[i].g = priors[3*i+1]*255.0; + cloud->points[i].b = priors[3*i+2]*255.0; + } + + pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); + cloud_sample->points.clear(); + for(unsigned int i = 0; i < current_point; i++){ + cloud_sample->points.push_back(cloud->points[i]); + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + + for(unsigned int i = 0; i < current_point; i++){ + cloud_sample->points[i].r = 255; + cloud_sample->points[i].g = 0; + cloud_sample->points[i].b = 0; + } + + for(unsigned int j = 0; j < todo.size(); j++){ + unsigned int i = todo[j]; + cloud_sample->points[i].r = 0; + cloud_sample->points[i].g = 255; + cloud_sample->points[i].b = 0; + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + } + + if(score < 0){continue;} if(current_label == 1){ labelID.back() = ++nr_obj_dyn; printf("Dynamic: %f -> %f\n",score,totsum); @@ -2592,7 +2761,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s sr.scores_moving.push_back(score); sr.total_moving.push_back(totsum); } - //printf("back: %i\n",labelID.back()); } } interframe_connectionId.clear(); @@ -2633,245 +2801,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } movemask.push_back(m); dynmask.push_back(d); - -// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frames[i]->rgb); -// cv::namedWindow( "moving", cv::WINDOW_AUTOSIZE ); cv::imshow( "moving", 255*(m > 0)); -// cv::namedWindow( "dynamic", cv::WINDOW_AUTOSIZE ); cv::imshow( "dynamic", 255*(d > 0)); -// cv::namedWindow( "dynamic2", cv::WINDOW_AUTOSIZE ); cv::imshow( "dynamic2", 255*(d2 > 0)); -// cv::waitKey(0); - } - - - -/* - std::vector staticdata; - std::vector dynamicdata; - std::vector movingdata; - pcl::PointCloud::Ptr staticcloud (new pcl::PointCloud); - pcl::PointCloud::Ptr dynamiccloud (new pcl::PointCloud); - pcl::PointCloud::Ptr movingcloud (new pcl::PointCloud); - - unsigned long current = 0; - for(unsigned long f = 0; f < frames.size(); f++){ - Camera * camera = frames[f]->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - unsigned short * depthdata = (unsigned short *)(frames[f]->depth.data); - - for(int j = 0; j < width*height; j++){ - if(depthdata[j] != 0){ - if(labels[current] == 0){ - staticdata.push_back(current); - staticcloud->points.push_back(cloud->pointmovemask,dynmask,s[current]); - } - - if(labels[current] == 1){ - dynamicdata.push_back(current); - dynamiccloud->points.push_back(cloud->points[current]); - } - - if(labels[current] == 2){ - movingdata.push_back(current); - movingcloud->points.push_back(cloud->points[current]); - } - } - current++; - } - } - - start_inf = getTime(); - - pcl::search::KdTree::Ptr dynamictree (new pcl::search::KdTree); - dynamictree->setInputCloud (dynamiccloud); - - std::vector dynamic_indices; - pcl::EuclideanClusterExtraction dynamic_ec; - dynamic_ec.setClusterTolerance (0.05); // 5cm - dynamic_ec.setMinClusterSize (1); - dynamic_ec.setMaxClusterSize (250000000); - dynamic_ec.setSearchMethod (dynamictree); - dynamic_ec.setInputCloud (dynamiccloud); - dynamic_ec.extract (dynamic_indices); - end_inf = getTime(); - printf("dynamic cluster time: %10.10fs\n",end_inf-start_inf); - for (unsigned int d = 0; d < dynamic_indices.size(); d++){ - double score = 0; - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_cluster2 (new pcl::PointCloud); - - double totsum = 0; - for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ - int pid = dynamic_indices[d].indices[ind]; - int dind = dynamicdata[pid]; - - double p0 = priors[3*dind+0]; - double p1 = priors[3*dind+1]; - double p2 = priors[3*dind+2]; - - double s = 0; - if(valids[dind]){ - if(p0 > p2){s += p1 - p0;} - else{ s += p1 - p2;} - score += s; - totsum++; - } - - pcl::PointXYZRGBNormal p = cloud->points[dind]; - p.r = 0;p.g = 0;p.b = 0; - if(s > 0){p.g = 255.0*s;} - if(s < 0){p.r = -255.0*s;} - - cloud_cluster->points.push_back(dynamiccloud->points[pid]); - cloud_cluster2->points.push_back(p); - } - - if(score > 200){ - printf("score: %f avg: %f\n",score,score/totsum); - printf("dynamic group found\n"); - for (unsigned int ind = 0; ind < dynamic_indices[d].indices.size(); ind++){ - labels[dynamicdata[dynamic_indices[d].indices[ind]]] = 3; - } - } - - if(false && cloud_cluster->points.size() > 500){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); - viewer->spin(); - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_cluster2, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster2), "cloud_cluster"); - viewer->spin(); - } - } - - start_inf = getTime(); - pcl::search::KdTree::Ptr movingtree (new pcl::search::KdTree); - movingtree->setInputCloud (movingcloud); - - std::vector moving_indices; - pcl::EuclideanClusterExtraction moving_ec; - moving_ec.setClusterTolerance (0.05); // 5cm - moving_ec.setMinClusterSize (1); - moving_ec.setMaxClusterSize (250000000); - moving_ec.setSearchMethod (movingtree);movemask,dynmask, - moving_ec.setInputCloud (movingcloud); - moving_ec.extract (moving_indices); - - end_inf = getTime(); - printf("moving cluster time: %10.10fs\n",end_inf-start_inf); - - for (unsigned int d = 0; d < moving_indices.size(); d++){ - double score = 0; - pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - pcl::PointCloud::Ptr cloud_cluster2 (new pcl::PointCloud); - - double totsum = 0; - double sum0 = 0; - double sum1 = 0; - double sum2 = 0; - for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ - int pid = moving_indices[d].indices[ind]; - int dind = movingdata[pid]; - - double p0 = priors[3*dind+0]; - double p1 = priors[3*dind+1]; - double p2 = priors[3*dind+2]; - - double s = 0; - if(valids[dind]){ - if(p1 > p2){s += p0 - p1;} - else{ s += p0 - p2;} - score += s; - totsum++; - sum0 += p0; - sum1 += p1; - sum2 += p2; - } - - pcl::PointXYZRGBNormal p = cloud->points[dind]; - p.r = 0;p.g = 0;p.b = 0; - if(s > 0){p.g = 255.0*s;} - if(s < 0){p.r = -255.0*s;} - - cloud_cluster->points.push_back(movingcloud->points[pid]); - cloud_cluster2->points.push_back(p); - } - - // printf("score: %f avg: %f\n",score,score/totsum); - // printf("sum: %f %f %f\n",sum0,sum1,sum2); - if(score > 200){ - for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ - labels[movingdata[moving_indices[d].indices[ind]]] = 4; - } - } - - if(false && cloud_cluster->points.size() > 500){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); - viewer->spin(); - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_cluster2, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster2), "cloud_cluster"); - viewer->spin(); - } - - - // double score = 0; - // pcl::PointCloud::Ptr cloud_cluster (new pcl::PointCloud); - // double sum0 = 0; - // double sum1 = 0; - // double sum2 = 0; - // for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ - // int pid = moving_indices[d].indices[ind]; - // int dind = movingdata[pid]; - // score += priors[3*dind+0] -0.5*(priors[3*dind+1]+priors[3*dind+2]); - - // sum0 += priors[3*dind+0]; - // sum1 += priors[3*dind+1]; - // sum2 += priors[3*dind+2]; - - // cloud_cluster->points.push_back(movingcloud->points[pid]); - // } - - // double avg = score/double(moving_indices[d].indices.size()); - //// printf("score: %f avg: %f\n",score,avg); - //// printf("sum0: %f sum1: %f sum2: %f\n",sum0,sum1,sum2); - // if(score > 100){ - // for (unsigned int ind = 0; ind < moving_indices[d].indices.size(); ind++){ - // labels[movingdata[moving_indices[d].indices[ind]]] = 4; - // } - // } - - //// if(cloud_cluster->points.size() > 500){ - //// viewer->removeAllPointClouds(); - //// viewer->addPointCloud (cloud_cluster, pcl::visualization::PointCloudColorHandlerRGBField(cloud_cluster), "cloud_cluster"); - //// viewer->spin(); - //// } } - - current = 0; - for(unsigned long i = 0; i < frames.size(); i++){ - Camera * camera = frames[i]->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - - cv::Mat m; - m.create(height,width,CV_8UC1); - unsigned char * mdata = (unsigned char*)m.data; - - cv::Mat d; - d.create(height,width,CV_8UC1); - unsigned char * ddata = (unsigned char*)d.data; - for(int j = 0; j < width*height; j++){ - mdata[j] = 255*(labels[current]==4); - ddata[j] = 255*(labels[current]==3); - current++; - } - movemask.push_back(m); - dynmask.push_back(d); - } -*/ if(debugg){ pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); @@ -2933,9 +2864,851 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s delete[] priors; printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime); - //return sr; } +/* +void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ + double computeMovingDynamicStatic_startTime = getTime(); + + SegmentationResults sr; + + int tot_nr_pixels = 0; + std::vector offsets; + + for(unsigned int i = 0; i < frames.size(); i++){ + offsets.push_back(tot_nr_pixels); + unsigned int nr_pixels = frames[i]->camera->width * frames[i]->camera->height; + tot_nr_pixels += nr_pixels; + } + + //Graph... + std::vector< std::vector > interframe_connectionId; + std::vector< std::vector > interframe_connectionStrength; + interframe_connectionId.resize(tot_nr_pixels); + interframe_connectionStrength.resize(tot_nr_pixels); + + + int current_point = 0; + float * priors = new float[3*tot_nr_pixels]; + bool * valids = new bool[tot_nr_pixels]; + + std::vector dvec; + std::vector nvec; + DistanceWeightFunction2 * dfunc; + DistanceWeightFunction2 * nfunc; + + double startTime = getTime(); + + std::vector< std::vector > framesp_test; + std::vector< std::vector > framesp; + for(unsigned int i = 0; i < frames.size(); i++){ + framesp_test.push_back(frames[i]->getSuperPoints(Eigen::Matrix4d::Identity(),10,false)); + framesp.push_back(frames[i]->getSuperPoints()); + } + printf("frames init time: %5.5fs\n",getTime()-startTime); + + startTime = getTime(); + std::vector< std::vector > bgsp; + for(unsigned int i = 0; i < bgcf.size(); i++){ + bgsp.push_back(bgcf[i]->getSuperPoints()); + } + printf("bg init time: %5.5fs\n",getTime()-startTime); + + startTime = getTime(); + // printf("computing all residuals\n"); + for(unsigned int i = 0; i < frames.size(); i++){ + std::vector & framesp1_test = framesp_test[i]; + std::vector & framesp1 = framesp[i]; + for(unsigned int j = 0; j < frames.size(); j++){ + if(i == j){continue;} + Eigen::Matrix4d p = poses[i].inverse() * poses[j]; + std::vector & framesp2 = framesp[j]; + getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp1_test,framesp1, 0, 0, frames[j],framesp2,offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); + } + } + + + // printf("training ppr\n"); + double dstdval = 0; + for(unsigned int i = 0; i < dvec.size(); i++){dstdval += dvec[i]*dvec[i];} + dstdval = sqrt(dstdval/double(dvec.size()-1)); + + DistanceWeightFunction2PPR2 * dfuncTMP = new DistanceWeightFunction2PPR2(); + dfunc = dfuncTMP; + dfuncTMP->refine_mean = true; + dfuncTMP->zeromean = false; + dfuncTMP->startreg = 0.0; + //dfuncTMP->startreg = 0.001; + //dfuncTMP->startreg = 0.0000001; + dfuncTMP->debugg_print = false; + dfuncTMP->bidir = true; + //dfuncTMP->bidir = false; + //dfuncTMP->maxp = 0.9; + dfuncTMP->maxp = 0.9; + dfuncTMP->maxd = 0.5; + dfuncTMP->histogram_size = 1000; + dfuncTMP->fixed_histogram_size = false;//true; + dfuncTMP->max_under_mean = false; + dfuncTMP->startmaxd = dfuncTMP->maxd; + dfuncTMP->starthistogram_size = dfuncTMP->histogram_size; + dfuncTMP->blurval = 0.5; + dfuncTMP->stdval2 = dstdval; + dfuncTMP->maxnoise = dstdval; + dfuncTMP->compute_infront = true; + dfuncTMP->reset(); + dfunc->computeModel(dvec); + double dthreshold = dfunc->getNoise(); + + GeneralizedGaussianDistribution * ggdnfunc = new GeneralizedGaussianDistribution(true,true); + ggdnfunc->nr_refineiters = 4; + DistanceWeightFunction2PPR3 * nfuncTMP = new DistanceWeightFunction2PPR3(ggdnfunc); + nfunc = nfuncTMP; + nfuncTMP->startreg = 0.00; + nfuncTMP->debugg_print = false; + nfuncTMP->bidir = false; + nfuncTMP->zeromean = true; + nfuncTMP->maxp = 0.999; + nfuncTMP->maxd = 1.0; + nfuncTMP->histogram_size = 1000; + nfuncTMP->fixed_histogram_size = true; + nfuncTMP->startmaxd = nfuncTMP->maxd; + nfuncTMP->starthistogram_size = nfuncTMP->histogram_size; + nfuncTMP->blurval = 0.5; + nfuncTMP->stdval2 = 1; + nfuncTMP->maxnoise = 1; + nfuncTMP->ggd = true; + nfuncTMP->reset(); + nfunc->computeModel(nvec); + + printf("training time: %5.5fs\n",getTime()-startTime); + + long frameConnections = 0; + std::vector< std::vector< std::vector > > pixel_weights; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + + double total_priortime = 0; + double total_connectiontime = 0; + double total_alloctime = 0; + double total_dealloctime = 0; + double total_Dynw = 0; + + for(unsigned int i = 0; i < frames.size(); i++){ + printf("currently workin on frame %i\n",i); + int offset = offsets[i]; + RGBDFrame * frame = frames[i]; + startTime = getTime(); + std::vector< std::vector > probs = getImageProbs(frame,9); + total_connectiontime += getTime()-startTime; + pixel_weights.push_back(probs); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + + Camera * camera = frame->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + const float idepth = camera->idepth_scale; + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + startTime = getTime(); + unsigned int nr_pixels = width * height; + double * current_overlaps = new double[nr_pixels]; + double * current_occlusions = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + current_overlaps[j] = 0; + current_occlusions[j] = 0; + } + + double * bg_overlaps = new double[nr_pixels]; + double * bg_occlusions = new double[nr_pixels]; + for(unsigned int j = 0; j < nr_pixels; j++){ + bg_overlaps[j] = 0; + bg_occlusions[j] = 0; + } + total_alloctime += getTime()-startTime; + + startTime = getTime(); + for(unsigned int j = 0; j < frames.size(); j++){ + if(i == j){continue;} + Eigen::Matrix4d p = poses[i].inverse() * poses[j]; + getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], current_overlaps, current_occlusions, frames[j],framesp[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); + } + + for(unsigned int j = 0; j < bgcf.size(); j++){ + Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; + getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], bg_overlaps, bg_occlusions, bgcf[j],bgsp[j],-1,-1,interframe_connectionId,interframe_connectionStrength,false); + } + total_Dynw += getTime()-startTime; + + + startTime = getTime(); + unsigned char * detdata = (unsigned char*)(frame->det_dilate.data); + float minprob = 0.01; + for(unsigned int h = 0; h < height;h++){ + for(unsigned int w = 0; w < width;w++){ + int ind = h*width+w; + + valids[offset+ind] = detdata[ind] == 0; + + float p_moving = current_occlusions[ind]; + float p_dynamic = std::max((bg_occlusions[ind]-p_moving)*current_overlaps[ind],0.0); + p_dynamic += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); + p_moving += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); + + float p_static = std::max(bg_overlaps[ind]-p_moving-p_dynamic,0.0); + + float leftover = 1-p_moving-p_dynamic-p_static; + float notMoving = current_overlaps[ind]; + + float bias = 0.0001; + + float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; + float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; + float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; + + float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover) + minprob; + float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover) + minprob; + float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover) + 2.0*minprob; + + priors[3*(offset+ind)+0] = p_moving_tot; + priors[3*(offset+ind)+1] = p_dynamic_tot; + priors[3*(offset+ind)+2] = p_static_tot; + + if(probs[0][ind] > 0.00000001){frameConnections++;} + if(probs[1][ind] > 0.00000001){frameConnections++;} + + current_point++; + } + } + + Eigen::Matrix4d p = poses[i]; + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + for(unsigned int h = 0; h < height;h++){ + for(unsigned int w = 0; w < width;w++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + float x = (float(w) - cx) * z * ifx; + float y = (float(h) - cy) * z * ify; + pcl::PointXYZRGBNormal point; + point.x = m00*x + m01*y + m02*z + m03; + point.y = m10*x + m11*y + m12*z + m13; + point.z = m20*x + m21*y + m22*z + m23; + point.r = priors[3*(offset+ind)+0]*255.0; + point.g = priors[3*(offset+ind)+1]*255.0; + point.b = priors[3*(offset+ind)+2]*255.0; + cloud->points.push_back(point); + } + } + total_priortime += getTime()-startTime; + + startTime = getTime(); + delete[] current_occlusions; + delete[] current_overlaps; + delete[] bg_occlusions; + delete[] bg_overlaps; + total_dealloctime += getTime()-startTime; + } + + delete dfuncTMP; + delete nfuncTMP; + + printf("total_priortime = %5.5fs\n", total_priortime); + printf("total_connectiontime = %5.5fs\n", total_connectiontime); + printf("total_alloctime = %5.5fs\n", total_alloctime); + printf("total_dealloctime = %5.5fs\n", total_dealloctime); + printf("total_Dynw = %5.5fs\n", total_Dynw); + + long interframeConnections = 0; + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + interframeConnections++; + } + } + + double start_inf = getTime(); + gc::Graph * g = new gc::Graph(current_point,frameConnections+interframeConnections); + for(unsigned long i = 0; i < current_point;i++){ + g -> add_node(); + double p_fg = priors[3*i+0]+priors[3*i+1]; + double p_bg = priors[3*i+2]; + double norm = p_fg + p_bg; + p_fg /= norm; + p_bg /= norm; + if(priors[3*i+0]+priors[3*i+1]+priors[3*i+2] <= 0){ + g -> add_tweights( i, 0, 0 ); + continue; + } + + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + g -> add_tweights( i, weightFG, weightBG ); + } + + double maxprob_same = 0.99999999999; + for(unsigned int i = 0; i < frames.size(); i++){ + int offset = offsets[i]; + Camera * camera = frames[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + std::vector< std::vector > & probs = pixel_weights[i]; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(w > 0 && probs[0][ind] > 0.00000001){ + int other = ind-1; + double p_same = std::min(double(probs[0][ind]),maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + g -> add_edge( ind+offset, other+offset, weight, weight ); + } + + if(h > 0 && probs[1][ind] > 0.00000001){ + int other = ind-width; + double p_same = std::min(double(probs[1][ind]),maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + g -> add_edge( ind+offset, other+offset, weight, weight ); + } + } + } + } + printf("run inference\n"); + + std::vector > interframe_connectionId_added; + interframe_connectionId_added.resize(interframe_connectionId.size()); + + std::vector > interframe_connectionStrength_added; + interframe_connectionStrength_added.resize(interframe_connectionStrength.size()); + + double tot_inf = 0; + for(unsigned int it = 0; it < 10; it++){ + double start_inf1 = getTime(); + + g -> maxflow(); + + double diffs = 0; + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + double weight = interframe_connectionStrength[i][j]; + unsigned long other = interframe_connectionId[i][j]; + if(weight > 1 && g->what_segment(i) != g->what_segment(other)){ + diffs++; + } + } + } + + if(diffs < 1000){break;} + + double adds = 100000; + double prob = std::min(adds / diffs,1.0); + printf("diffs: %f adds: %f prob: %f\n",diffs,adds,prob); + + double trueadds = 0; + + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + double weight = interframe_connectionStrength[i][j]; + unsigned long other = interframe_connectionId[i][j]; + if(weight > 0.1 && g->what_segment(i) != g->what_segment(other)){ + if(rand() <= prob*RAND_MAX){ + trueadds++; + g -> add_edge( i, other, weight, weight ); + + interframe_connectionId_added[i].push_back(other); + interframe_connectionStrength_added[i].push_back(weight); + + interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back(); + interframe_connectionStrength[i].pop_back(); + + interframe_connectionId[i][j] = interframe_connectionId[i].back(); + interframe_connectionId[i].pop_back(); + j--; + } + } + } + } + printf("trueadds: %f\n",trueadds); + + tot_inf += getTime()-start_inf1; + printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf); + + if(tot_inf > 90){break;} + } + + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId_added[i].size();j++){ + interframe_connectionStrength[i].push_back(interframe_connectionStrength_added[i][j]); + interframe_connectionId[i].push_back(interframe_connectionId_added[i][j]); + } + } + + int dynamic_label = 1; + std::vector labels; + std::vector dyn_ind; + std::vector stat_ind; + labels.resize(current_point); + stat_ind.resize(current_point); + long nr_dynamic = 0; + for(unsigned long i = 0; i < current_point;i++){ + labels[i] = g->what_segment(i); + nr_dynamic += labels[i]==dynamic_label; + if(labels[i]==dynamic_label){ + stat_ind[i] = dyn_ind.size(); + dyn_ind.push_back(i); + } + } + delete g; + + double end_inf = getTime(); + printf("static inference time: %10.10fs\n",end_inf-start_inf); + + long dynamic_frameConnections = 0; + + for(unsigned long i = 0; i < frames.size(); i++){ + int offset = offsets[i]; + Camera * camera = frames[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + std::vector< std::vector > & probs = pixel_weights[i]; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(labels[ind+offset] == dynamic_label){ + if(w > 0 && probs[0][ind] > 0.00000001){ + int other = ind-1; + if(labels[ind+offset] == labels[other+offset]){dynamic_frameConnections++;} + } + + if(h > 0 && probs[1][ind] > 0.00000001){ + int other = ind-width; + if(labels[ind+offset] == labels[other+offset]){dynamic_frameConnections++;} + } + } + } + } + } + + long dynamic_interframeConnections = 0; + for(unsigned long i = 0; i < current_point;i++){ + if(labels[i] == dynamic_label){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + if(labels[interframe_connectionId[i][j]] == labels[i]){dynamic_interframeConnections++;} + } + } + } + + start_inf = getTime(); + gc::Graph * dynamic_g = new gc::Graph(nr_dynamic,dynamic_frameConnections+dynamic_interframeConnections); + for(unsigned long i = 0; i < dyn_ind.size();i++){ + dynamic_g -> add_node(); + double p_fg = priors[3*dyn_ind[i]+0]; + double p_bg = priors[3*dyn_ind[i]+1]; + double norm = p_fg+p_bg; + p_fg /= norm; + p_bg /= norm; + if(norm <= 0){ + dynamic_g -> add_tweights( i, 0, 0 ); + continue; + } + + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + dynamic_g -> add_tweights( i, weightFG, weightBG ); + } + + for(unsigned long i = 0; i < frames.size(); i++){ + int offset = offsets[i]; + Camera * camera = frames[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + std::vector< std::vector > & probs = pixel_weights[i]; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(labels[ind+offset] == dynamic_label){ + if(w > 0 && probs[0][ind] > 0.00000001){ + int other = ind-1; + if(labels[ind+offset] == labels[other+offset]){ + //dynamic_frameConnections++; + double p_same = std::min(double(probs[0][ind]),maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight ); + } + } + + if(h > 0 && probs[1][ind] > 0.00000001){ + int other = ind-width; + if(labels[ind+offset] == labels[other+offset]){ + //dynamic_frameConnections++; + double p_same = std::min(double(probs[0][ind]),maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight ); + } + } + } + } + } + } + + for(unsigned long i = 0; i < current_point;i++){ + if(labels[i] == dynamic_label){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + if(labels[interframe_connectionId[i][j]] == labels[i]){ + double weight = interframe_connectionStrength[i][j]; + dynamic_g -> add_edge( stat_ind[i], stat_ind[ interframe_connectionId[i][j] ], weight, weight ); + } + } + } + } + dynamic_g -> maxflow(); + + + tot_inf = 0; + for(unsigned int it = 0; false && it < 100; it++){ + double start_inf1 = getTime(); + dynamic_g -> maxflow(); + + double diffs = 0; + for(unsigned long i = 0; i < current_point;i++){ + if(labels[i] == dynamic_label){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + if(labels[interframe_connectionId[i][j]] == labels[i]){ + double weight = interframe_connectionStrength[i][j]; + + unsigned long i2 = stat_ind[i]; + unsigned long j2 = stat_ind[ interframe_connectionId[i][j] ]; + if(weight > 1 && g->what_segment(i2) != g->what_segment(j2)){ + diffs++; + } + } + } + } + } + + + double adds = 100000; + double prob = std::min(adds / diffs,1.0); + printf("diffs: %f adds: %f prob: %f\n",diffs,adds,prob); + + if(diffs < 1000){break;} + + + double trueadds = 0; + + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + if(labels[i] == dynamic_label){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + if(labels[interframe_connectionId[i][j]] == labels[i]){ + + double weight = interframe_connectionStrength[i][j]; + unsigned long other = interframe_connectionId[i][j]; + + unsigned long i2 = stat_ind[i]; + unsigned long j2 = stat_ind[ interframe_connectionId[i][j] ]; + if(weight > 1 && g->what_segment(i2) != g->what_segment(j2)){ + if(rand() <= prob*RAND_MAX){ + trueadds++; + g -> add_edge( i2, j2, weight, weight ); + + interframe_connectionId_added[i].push_back(other); + interframe_connectionStrength_added[i].push_back(weight); + + interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back(); + interframe_connectionStrength[i].pop_back(); + + interframe_connectionId[i][j] = interframe_connectionId[i].back(); + interframe_connectionId[i].pop_back(); + j--; + } + } + } + } + } + } + printf("trueadds: %f\n",trueadds); + + tot_inf += getTime()-start_inf1; + printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf); + if(tot_inf > 90){break;} + } + + end_inf = getTime(); + printf("dynamic inference time: %10.10fs\n",end_inf-start_inf); + + long nr_moving = 0; + for(unsigned long i = 0; i < dyn_ind.size();i++){ + int res = dynamic_g->what_segment(i); + if(res == 1){ + labels[dyn_ind[i]] = 2; + nr_moving++; + } + } + delete dynamic_g; + + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId_added[i].size();j++){ + interframe_connectionStrength[i].push_back(interframe_connectionStrength_added[i][j]); + interframe_connectionId[i].push_back(interframe_connectionId_added[i][j]); + } + } + + const unsigned int nr_frames = frames.size(); + const unsigned int width = frames[0]->camera->width; + const unsigned int height = frames[0]->camera->height; + const unsigned int pixels_per_image = width*height; + const unsigned int nr_pixels = nr_frames*pixels_per_image; + + double probthresh = 0.5; + double str_probthresh = -log(probthresh); + unsigned int number_of_dynamics = 0; + unsigned int nr_obj_dyn = 0; + unsigned int nr_obj_mov = 0; + std::vector objectlabel; + std::vector labelID; + labelID.push_back(0); + objectlabel.resize(nr_pixels); + for(unsigned long i = 0; i < nr_pixels; i++){objectlabel[i] = 0;} + for(unsigned long ind = 0; ind < nr_pixels; ind++){ + if(valids[ind] && objectlabel[ind] == 0 && labels[ind] != 0){ + unsigned int current_label = labels[ind]; + number_of_dynamics++; + objectlabel[ind] = number_of_dynamics; + unsigned long todocounter = 0; + std::vector< unsigned long > todo; + todo.push_back(ind); + double score = 0; + double totsum = 0; + while(todocounter < todo.size()){ + unsigned long cind = todo[todocounter++]; + unsigned long frameind = cind / pixels_per_image; + + + unsigned long iind = cind % pixels_per_image; + unsigned long w = iind % width; + unsigned long h = iind / width; + + double p0 = priors[3*cind+0]; + double p1 = priors[3*cind+1]; + double p2 = priors[3*cind+2]; + + if(valids[cind]){ + double s = 0; + if(p0 > p2){s += p1 - p0;} + else{ s += p1 - p2;} + score += s; + totsum++; + } + + float * dedata = (float*)(frames[frameind]->de.data); + + int dir; + dir = -1; + if( w > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + dir = 1; + if( w < (width-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + dir = -width; + if( h > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + dir = width; + if( h < (height-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + for(unsigned long j = 0; j < interframe_connectionId[cind].size();j++){ + unsigned long other = interframe_connectionId[cind][j]; + if(interframe_connectionStrength[cind][j] > str_probthresh && objectlabel[other] == 0 && labels[other] == current_label){ + objectlabel[other] = number_of_dynamics; + todo.push_back(other); + + double d = sqrt(pow(cloud->points[cind].x-cloud->points[other].x,2)+pow(cloud->points[cind].y-cloud->points[other].y,2)+pow(cloud->points[cind].z-cloud->points[other].z,2)); + if(d > 0.15){ + printf("d: %f -> %f\n",d,exp(-interframe_connectionStrength[cind][j])); + } + } + } + } + + printf("score: %f\n",score); + + if(debugg && todo.size() > 10000){ + + + printf("3-class\n"); + for(unsigned int i = 0; i < current_point; i++){ + cloud->points[i].r = priors[3*i+0]*255.0; + cloud->points[i].g = priors[3*i+1]*255.0; + cloud->points[i].b = priors[3*i+2]*255.0; + } + + pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); + cloud_sample->points.clear(); + for(unsigned int i = 0; i < current_point; i++){ + //if(rand() % 4 == 0){ + cloud_sample->points.push_back(cloud->points[i]); + //} + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + + for(unsigned int i = 0; i < current_point; i++){ + cloud_sample->points[i].r = 255; + cloud_sample->points[i].g = 0; + cloud_sample->points[i].b = 0; + } + + for(unsigned int j = 0; j < todo.size(); j++){ + unsigned int i = todo[j]; + cloud_sample->points[i].r = 0; + cloud_sample->points[i].g = 255; + cloud_sample->points[i].b = 0; + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + } + + labelID.push_back(0); + //if(score < 200){continue;} + + if(score < 0){continue;} + if(current_label == 1){ + labelID.back() = ++nr_obj_dyn; + printf("Dynamic: %f -> %f\n",score,totsum); + sr.component_dynamic.push_back(todo); + sr.scores_dynamic.push_back(score); + sr.total_dynamic.push_back(totsum); + } + if(current_label == 2){ + labelID.back() = --nr_obj_mov; + printf("Moving: %f -> %f\n",score,totsum); + sr.component_moving.push_back(todo); + sr.scores_moving.push_back(score); + sr.total_moving.push_back(totsum); + } + } + } + interframe_connectionId.clear(); + interframe_connectionStrength.clear(); + + printf("connectedComponent: %5.5fs\n",getTime()-start_inf); + + int current = 0; + for(unsigned long i = 0; i < frames.size(); i++){ + Camera * camera = frames[i]->camera; + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + cv::Mat m; + m.create(height,width,CV_8UC1); + unsigned char * mdata = (unsigned char*)m.data; + + cv::Mat d; + d.create(height,width,CV_8UC1); + unsigned char * ddata = (unsigned char*)d.data; + + cv::Mat d2; + d2.create(height,width,CV_8UC1); + unsigned char * ddata2 = (unsigned char*)d2.data; + + for(int j = 0; j < width*height; j++){ + mdata[j] = 0; + ddata[j] = 0; + ddata2[j] = labels[current]; + unsigned int label = objectlabel[current]; + int lid = labelID[label]; + if(lid > 0){ + ddata[j] = lid; + }else if(lid < 0){ + mdata[j] = -lid; + } + current++; + } + movemask.push_back(m); + dynmask.push_back(d); + } + + if(debugg){ + pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); + + printf("3-class\n"); + for(unsigned int i = 0; i < current_point; i++){ + cloud->points[i].r = priors[3*i+0]*255.0; + cloud->points[i].g = priors[3*i+1]*255.0; + cloud->points[i].b = priors[3*i+2]*255.0; + } + + cloud_sample->points.clear(); + for(unsigned int i = 0; i < current_point; i++){ + if(rand() % 4 == 0){ + cloud_sample->points.push_back(cloud->points[i]); + } + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + + + cloud_sample->points.clear(); + for(unsigned int i = 0; i < current_point; i++){ + if(rand() % 4 == 0 && labels[i] == 0){ + cloud_sample->points.push_back(cloud->points[i]); + cloud_sample->points.back().r = 0; + cloud_sample->points.back().g = 0; + cloud_sample->points.back().b = 255; + } + } + + for(unsigned int c = 0; c < sr.component_dynamic.size(); c++){ + int randr = rand()%256; + int randg = rand()%256; + int randb = rand()%256; + for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){ + cloud_sample->points.push_back(cloud->points[sr.component_dynamic[c][i]]); + cloud_sample->points.back().r = randr; + cloud_sample->points.back().g = randg; + cloud_sample->points.back().b = randb; + } + } + + for(unsigned int c = 0; c < sr.component_moving.size(); c++){ + int randr = rand()%256; + int randg = rand()%256; + int randb = rand()%256; + for(unsigned int i = 0; i < sr.component_moving[c].size(); i++){ + cloud_sample->points.push_back(cloud->points[sr.component_moving[c][i]]); + cloud_sample->points.back().r = randr; + cloud_sample->points.back().g = randg; + cloud_sample->points.back().b = randb; + } + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + } + + delete[] priors; + printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime); +} +*/ vector ModelUpdater::computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg){ // @@ -3508,9 +4281,9 @@ std::vector ModelUpdater::computeDynamicObject(reglib::Model * bg, Eig unsigned char * internaldata = (unsigned char *)(internalmask.data); for(unsigned int i = 0; i < width*height;i++){internaldata[i] = 255.0*(g->what_segment(i) == gc::Graph::SOURCE);} -// cv::imshow( "rgb", cf[i]->rgb ); -// cv::imshow( "internalmask", internalmask ); -// cv::waitKey(30); + // cv::imshow( "rgb", cf[i]->rgb ); + // cv::imshow( "internalmask", internalmask ); + // cv::waitKey(30); newmasks.push_back(internalmask); //Time to compute external masks... diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 65fd7853..81159a7a 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -98,90 +98,90 @@ MassRegistrationPPR2::~MassRegistrationPPR2(){ delete[] kp_Qn_arr; delete[] kp_Xp_arr; delete[] kp_Xn_arr; - delete[] kp_rangeW_arr; + delete[] kp_rangeW_arr; - delete[] depthedge_Qp_arr; - delete[] depthedge_Xp_arr; - delete[] depthedge_rangeW_arr; + delete[] depthedge_Qp_arr; + delete[] depthedge_Xp_arr; + delete[] depthedge_rangeW_arr; - clearData(); + clearData(); } void MassRegistrationPPR2::addModel(Model * model){ - double total_load_time_start = getTime(); + double total_load_time_start = getTime(); - nr_matches.push_back( 0); + nr_matches.push_back( 0); matchids.push_back( std::vector< std::vector >() ); - matchdists.push_back( std::vector< std::vector >() ); - nr_datas.push_back( 0); + matchdists.push_back( std::vector< std::vector >() ); + nr_datas.push_back( 0); - points.push_back( Eigen::Matrix()); - colors.push_back( Eigen::Matrix()); - normals.push_back( Eigen::Matrix()); - transformed_points.push_back( Eigen::Matrix()); - transformed_normals.push_back( Eigen::Matrix()); + points.push_back( Eigen::Matrix()); + colors.push_back( Eigen::Matrix()); + normals.push_back( Eigen::Matrix()); + transformed_points.push_back( Eigen::Matrix()); + transformed_normals.push_back( Eigen::Matrix()); - informations.push_back( Eigen::VectorXd()); + informations.push_back( Eigen::VectorXd()); - nr_arraypoints.push_back(0); - arraypoints.push_back(0); - arraynormals.push_back(0); - arraycolors.push_back(0); - arrayinformations.push_back(0); - trees3d.push_back(0); - a3dv.push_back(0); + nr_arraypoints.push_back(0); + arraypoints.push_back(0); + arraynormals.push_back(0); + arraycolors.push_back(0); + arrayinformations.push_back(0); + trees3d.push_back(0); + a3dv.push_back(0); depthedge_nr_matches.push_back( 0); depthedge_matchids.push_back( std::vector< std::vector >() ); depthedge_matchdists.push_back( std::vector< std::vector >() ); - depthedge_nr_arraypoints.push_back(0); - depthedge_arraypoints.push_back(0); - depthedge_arrayinformations.push_back(0); - depthedge_trees3d.push_back(0); - depthedge_a3dv.push_back(0); + depthedge_nr_arraypoints.push_back(0); + depthedge_arraypoints.push_back(0); + depthedge_arrayinformations.push_back(0); + depthedge_trees3d.push_back(0); + depthedge_a3dv.push_back(0); long step = maskstep*maskstep; - is_ok.push_back(false); + is_ok.push_back(false); long count = model->points.size()/step; long i = points.size()-1; - if(count < 10){ - is_ok[i] = false; - return; - }else{ - is_ok[i] = true; - } - -// printf("%i is ok %i\n",i,is_ok[i]); - - nr_datas[i] = count; - points[i].resize(Eigen::NoChange,count); - colors[i].resize(Eigen::NoChange,count); - normals[i].resize(Eigen::NoChange,count); - transformed_points[i].resize(Eigen::NoChange,count); - transformed_normals[i].resize(Eigen::NoChange,count); - - double * ap = new double[3*count]; - double * an = new double[3*count]; - double * ac = new double[3*count]; - double * ai = new double[3*count]; - nr_arraypoints[i] = count; - arraypoints[i] = ap; - arraynormals[i] = an; - arraycolors[i] = ac; - arrayinformations[i] = ai; - - Eigen::Matrix & X = points[i]; - Eigen::Matrix & C = colors[i]; - Eigen::Matrix & Xn = normals[i]; - Eigen::Matrix & tX = transformed_points[i]; - Eigen::Matrix & tXn = transformed_normals[i]; - Eigen::VectorXd information (count); + if(count < 10){ + is_ok[i] = false; + return; + }else{ + is_ok[i] = true; + } + + // printf("%i is ok %i\n",i,is_ok[i]); + + nr_datas[i] = count; + points[i].resize(Eigen::NoChange,count); + colors[i].resize(Eigen::NoChange,count); + normals[i].resize(Eigen::NoChange,count); + transformed_points[i].resize(Eigen::NoChange,count); + transformed_normals[i].resize(Eigen::NoChange,count); + + double * ap = new double[3*count]; + double * an = new double[3*count]; + double * ac = new double[3*count]; + double * ai = new double[3*count]; + nr_arraypoints[i] = count; + arraypoints[i] = ap; + arraynormals[i] = an; + arraycolors[i] = ac; + arrayinformations[i] = ai; + + Eigen::Matrix & X = points[i]; + Eigen::Matrix & C = colors[i]; + Eigen::Matrix & Xn = normals[i]; + Eigen::Matrix & tX = transformed_points[i]; + Eigen::Matrix & tXn = transformed_normals[i]; + Eigen::VectorXd information (count); for(unsigned long c = 0; c < count; c++){ superpoint & sp = model->points[c*step]; @@ -212,86 +212,86 @@ void MassRegistrationPPR2::addModel(Model * model){ C(2,c) = ac[3*c+2]; -// ap[3*c+0] = model->points[c*step].point(0); -// ap[3*c+1] = model->points[c*step].point(1); -// ap[3*c+2] = model->points[c*step].point(2); + // ap[3*c+0] = model->points[c*step].point(0); + // ap[3*c+1] = model->points[c*step].point(1); + // ap[3*c+2] = model->points[c*step].point(2); -// an[3*c+0] = model->points[c*step].normal(0); -// an[3*c+1] = model->points[c*step].normal(1); -// an[3*c+2] = model->points[c*step].normal(2); + // an[3*c+0] = model->points[c*step].normal(0); + // an[3*c+1] = model->points[c*step].normal(1); + // an[3*c+2] = model->points[c*step].normal(2); -// ac[3*c+0] =model->points[c*step].feature(0); -// ac[3*c+1] =model->points[c*step].feature(1); -// ac[3*c+2] =model->points[c*step].feature(2); + // ac[3*c+0] =model->points[c*step].feature(0); + // ac[3*c+1] =model->points[c*step].feature(1); + // ac[3*c+2] =model->points[c*step].feature(2); -// ai[c] = 1.0/model->points[c*step].point_information;//1.0/(z*z); + // ai[c] = 1.0/model->points[c*step].point_information;//1.0/(z*z); -// X(0,c) = ap[3*c+0]; -// X(1,c) = ap[3*c+1]; -// X(2,c) = ap[3*c+2]; -// Xn(0,c) = an[3*c+0]; -// Xn(1,c) = an[3*c+1]; -// Xn(2,c) = an[3*c+2]; + // X(0,c) = ap[3*c+0]; + // X(1,c) = ap[3*c+1]; + // X(2,c) = ap[3*c+2]; + // Xn(0,c) = an[3*c+0]; + // Xn(1,c) = an[3*c+1]; + // Xn(2,c) = an[3*c+2]; -// information(c) = ai[c];//1.0/(z*z); -// C(0,c) = ac[3*c+0]; -// C(1,c) = ac[3*c+1]; -// C(2,c) = ac[3*c+2]; + // information(c) = ai[c];//1.0/(z*z); + // C(0,c) = ac[3*c+0]; + // C(1,c) = ac[3*c+1]; + // C(2,c) = ac[3*c+2]; } - informations[i] = information; + informations[i] = information; - ArrayData3D * a3d = new ArrayData3D; - a3d->data = ap; - a3d->rows = count; - a3dv[i] = a3d; - trees3d[i] = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); - trees3d[i]->buildIndex(); + ArrayData3D * a3d = new ArrayData3D; + a3d->data = ap; + a3d->rows = count; + a3dv[i] = a3d; + trees3d[i] = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + trees3d[i]->buildIndex(); -/* + /* long depthedge_count = 0; for(unsigned long w = 0; w < width; w++){ for(unsigned long h = 0; h < height; h++){ long ind = h*width+w; - if(maskvec[ind] && edgedata[ind] == 255){ - float z = idepth*float(depthdata[ind]); - if(z > 0.2){depthedge_count++;} - } - } - } - - if(depthedge_count < 10){ - double * depthedge_ap = new double[3*depthedge_count]; - double * depthedge_ai = new double[3*depthedge_count]; - depthedge_nr_arraypoints[i] = depthedge_count; - depthedge_arraypoints[i] = depthedge_ap; - depthedge_arrayinformations[i] = depthedge_ai; - - c = 0; + if(maskvec[ind] && edgedata[ind] == 255){ + float z = idepth*float(depthdata[ind]); + if(z > 0.2){depthedge_count++;} + } + } + } + + if(depthedge_count < 10){ + double * depthedge_ap = new double[3*depthedge_count]; + double * depthedge_ai = new double[3*depthedge_count]; + depthedge_nr_arraypoints[i] = depthedge_count; + depthedge_arraypoints[i] = depthedge_ap; + depthedge_arrayinformations[i] = depthedge_ai; + + c = 0; for(unsigned long w = 0; w < width; w++){ for(unsigned long h = 0; h < height; h++){ - if(c == depthedge_count){continue;} + if(c == depthedge_count){continue;} long ind = h*width+w; - if(maskvec[ind] && edgedata[ind] == 255){ - float z = idepth*float(depthdata[ind]); - if(z > 0.2){ - depthedge_ap[3*c+0] = (w - cx) * z * ifx; - depthedge_ap[3*c+1] = (h - cy) * z * ify;; - depthedge_ap[3*c+2] = z; - depthedge_ai[c] = pow(fabs(z),-2);//1.0/(z*z); - c++; - } - } - } - } - - ArrayData3D * depthedge_a3d = new ArrayData3D; - depthedge_a3d->data = depthedge_ap; - depthedge_a3d->rows = depthedge_count; - depthedge_a3dv[i] = depthedge_a3d; - depthedge_trees3d[i] = new Tree3d(3, *depthedge_a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); - depthedge_trees3d[i]->buildIndex(); - } + if(maskvec[ind] && edgedata[ind] == 255){ + float z = idepth*float(depthdata[ind]); + if(z > 0.2){ + depthedge_ap[3*c+0] = (w - cx) * z * ifx; + depthedge_ap[3*c+1] = (h - cy) * z * ify;; + depthedge_ap[3*c+2] = z; + depthedge_ai[c] = pow(fabs(z),-2);//1.0/(z*z); + c++; + } + } + } + } + + ArrayData3D * depthedge_a3d = new ArrayData3D; + depthedge_a3d->data = depthedge_ap; + depthedge_a3d->rows = depthedge_count; + depthedge_a3dv[i] = depthedge_a3d; + depthedge_trees3d[i] = new Tree3d(3, *depthedge_a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); + depthedge_trees3d[i]->buildIndex(); + } */ //printf("addModel total load time: %5.5f points: %6.6i\n",getTime()-total_load_time_start,count); } @@ -305,86 +305,86 @@ void MassRegistrationPPR2::addModelData(Model * model_, bool submodels){ addData(model->submodels[i]->getPCLnormalcloud(1,false)); } }else{ - //setData(model->frames,model->modelmasks); + //setData(model->frames,model->modelmasks); for(unsigned long i = 0; i < model->submodels.size(); i++){ - addData(model->submodels[i]->getPCLnormalcloud(1,false)); -// kp_nr_arraypoints.push_back(0); -// kp_arraypoints.push_back(0); -// kp_arraynormals.push_back(0); -// kp_arrayinformations.push_back(0); -// kp_arraydescriptors.push_back(0); -// frameid.push_back(-1); - } + addData(model->submodels[i]->getPCLnormalcloud(1,false)); + // kp_nr_arraypoints.push_back(0); + // kp_arraypoints.push_back(0); + // kp_arraynormals.push_back(0); + // kp_arrayinformations.push_back(0); + // kp_arraydescriptors.push_back(0); + // frameid.push_back(-1); + } unsigned long nr_frames = model->frames.size(); for(unsigned long i = 0; i < nr_frames; i++){ -// frameid.push_back(i); - addData(model->frames[i], model->modelmasks[i]); - -// RGBDFrame* frame = model->frames[i]; -// std::vector & keypoints = model->all_keypoints[i]; -// cv::Mat & descriptors = model->all_descriptors[i]; -// //uint64_t * descriptorsdata = (uint64_t *)(descriptors.data); - - -// float * normalsdata = (float *)(frame->normals.data); -// unsigned short * depthdata = (unsigned short *)(frame->depth.data); - -// Camera * camera = frame->camera; -// const unsigned long width = camera->width; -// const unsigned long height = camera->height; -// const float idepth = camera->idepth_scale; -// const float cx = camera->cx; -// const float cy = camera->cy; -// const float ifx = 1.0/camera->fx; -// const float ify = 1.0/camera->fy; - -// unsigned long nr_kp = keypoints.size(); -// if(nr_kp){ -// double * ap = new double[3*nr_kp]; -// double * an = new double[3*nr_kp]; -// double * ai = new double[nr_kp]; -// for(unsigned long k = 0; k < nr_kp; k++){ -// cv::KeyPoint & kp = keypoints[k]; -// double w = kp.pt.x; -// double h = kp.pt.y; -// long ind = int(h+0.5)*width+int(w+0.5); - -// float z = idepth*float(depthdata[ind]); -// float x = (w - cx) * z * ifx; -// float y = (h - cy) * z * ify; - -// ap[3*k+0] =x; -// ap[3*k+1] =y; -// ap[3*k+2] =z; - -// an[3*k+0] = normalsdata[3*ind+0]; -// an[3*k+1] = normalsdata[3*ind+1]; -// an[3*k+2] = normalsdata[3*ind+2]; - -// ai[k] = pow(fabs(z),-2); -// } - -// kp_nr_arraypoints.push_back(nr_kp); -// kp_arraypoints.push_back(ap); -// kp_arraynormals.push_back(an); -// kp_arrayinformations.push_back(ai); -// kp_arraydescriptors.push_back((uint64_t *)(descriptors.data)); -// }else{ -// kp_nr_arraypoints.push_back(0); -// kp_arraypoints.push_back(0); -// kp_arraynormals.push_back(0); -// kp_arrayinformations.push_back(0); -// kp_arraydescriptors.push_back(0); -// } + // frameid.push_back(i); + addData(model->frames[i], model->modelmasks[i]); + + // RGBDFrame* frame = model->frames[i]; + // std::vector & keypoints = model->all_keypoints[i]; + // cv::Mat & descriptors = model->all_descriptors[i]; + // //uint64_t * descriptorsdata = (uint64_t *)(descriptors.data); + + + // float * normalsdata = (float *)(frame->normals.data); + // unsigned short * depthdata = (unsigned short *)(frame->depth.data); + + // Camera * camera = frame->camera; + // const unsigned long width = camera->width; + // const unsigned long height = camera->height; + // const float idepth = camera->idepth_scale; + // const float cx = camera->cx; + // const float cy = camera->cy; + // const float ifx = 1.0/camera->fx; + // const float ify = 1.0/camera->fy; + + // unsigned long nr_kp = keypoints.size(); + // if(nr_kp){ + // double * ap = new double[3*nr_kp]; + // double * an = new double[3*nr_kp]; + // double * ai = new double[nr_kp]; + // for(unsigned long k = 0; k < nr_kp; k++){ + // cv::KeyPoint & kp = keypoints[k]; + // double w = kp.pt.x; + // double h = kp.pt.y; + // long ind = int(h+0.5)*width+int(w+0.5); + + // float z = idepth*float(depthdata[ind]); + // float x = (w - cx) * z * ifx; + // float y = (h - cy) * z * ify; + + // ap[3*k+0] =x; + // ap[3*k+1] =y; + // ap[3*k+2] =z; + + // an[3*k+0] = normalsdata[3*ind+0]; + // an[3*k+1] = normalsdata[3*ind+1]; + // an[3*k+2] = normalsdata[3*ind+2]; + + // ai[k] = pow(fabs(z),-2); + // } + + // kp_nr_arraypoints.push_back(nr_kp); + // kp_arraypoints.push_back(ap); + // kp_arraynormals.push_back(an); + // kp_arrayinformations.push_back(ai); + // kp_arraydescriptors.push_back((uint64_t *)(descriptors.data)); + // }else{ + // kp_nr_arraypoints.push_back(0); + // kp_arraypoints.push_back(0); + // kp_arraynormals.push_back(0); + // kp_arrayinformations.push_back(0); + // kp_arraydescriptors.push_back(0); + // } } -// for(long i = 0; i < is_ok.size(); i++){ -// if(is_ok[i]){printf("%i is ok\n",i); -// }else{printf("%i is not ok\n",i);} -// } + // for(long i = 0; i < is_ok.size(); i++){ + // if(is_ok[i]){printf("%i is ok\n",i); + // }else{printf("%i is not ok\n",i);} + // } } @@ -410,9 +410,9 @@ void MassRegistrationPPR2::setData(std::vector< pcl::PointCloudgetNoise(),2); - double good = 0; - double bad = 0; + double threshold = pow(10*f->getNoise(),2); + double good = 0; + double bad = 0; #if defdomultithread - #pragma omp parallel for num_threads(8) +#pragma omp parallel for num_threads(8) #endif for(unsigned long k = 0; k < nr_ap; ++k) { long prev = matchid[k]; @@ -567,10 +567,10 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ resultSet.init(&ret_index, &out_dist_sqr ); t3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); - if(out_dist_sqr < threshold){ - good++; - }else{ - bad++; + if(out_dist_sqr < threshold){ + good++; + }else{ + bad++; } long current = ret_index; new_good_rematches += prev != current; @@ -578,7 +578,7 @@ double matchframes(DistanceWeightFunction2PPR2 * f, Eigen::Affine3d rp, long nr_ matchid[k] = current; matchdist[k] = out_dist_sqr; } - return good/(good+bad+0.001); + return good/(good+bad+0.001); } void MassRegistrationPPR2::rematch(std::vector poses, std::vector prev_poses, bool rematch_surface, bool rematch_edges, bool first){ @@ -602,13 +602,13 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect for(unsigned long j = 0; j < nr_frames; j++){ if(!is_ok[j]){continue;} if(i == j){continue;} - if((matchscores[i][j] <= 0.001) && (matchscores[j][i] <= 0.001) && (rand()%nr_frames > 1)){ - ignores_overlap++; - continue; - } + if((matchscores[i][j] <= 0.001) && (matchscores[j][i] <= 0.001) && (rand()%nr_frames > 1)){ + ignores_overlap++; + continue; + } Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); - if(false && !first){ + if(false && !first){ Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); Eigen::Affine3d diff = prev_rp.inverse()*rp; @@ -624,21 +624,21 @@ void MassRegistrationPPR2::rematch(std::vector poses, std::vect } change_trans += sqrt(dt); - if(change_trans < 1.0*stopval && change_rot < 1.0*stopval){ignores_motion++;continue;} + if(change_trans < 1.0*stopval && change_rot < 1.0*stopval){ignores_motion++;continue;} } - double ratiosum = 0; + double ratiosum = 0; if(rematch_edges && depthedge_nr_arraypoints[i] > 10 && depthedge_nr_arraypoints[j] > 10){ - ratiosum += matchframes(depthedge_func,rp, depthedge_nr_arraypoints[i], depthedge_arraypoints[i], depthedge_matchids[i][j], depthedge_matchdists[i][j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); + ratiosum += matchframes(depthedge_func,rp, depthedge_nr_arraypoints[i], depthedge_arraypoints[i], depthedge_matchids[i][j], depthedge_matchdists[i][j],depthedge_trees3d[j], new_good_rematches,new_total_rematches); } if(rematch_surface && nr_arraypoints[i] > 10 && nr_arraypoints[j] > 10){ - ratiosum += matchframes(func,rp, nr_arraypoints[i], arraypoints[i], matchids[i][j], matchdists[i][j],trees3d[j], new_good_rematches,new_total_rematches); + ratiosum += matchframes(func,rp, nr_arraypoints[i], arraypoints[i], matchids[i][j], matchdists[i][j],trees3d[j], new_good_rematches,new_total_rematches); } - matchscores[i][j] = ratiosum; - overlapping += ratiosum > 0; + matchscores[i][j] = ratiosum; + overlapping += ratiosum > 0; work_done++; } } @@ -752,170 +752,101 @@ Eigen::MatrixXd MassRegistrationPPR2::getAllResiduals(std::vector & matchidi = matchids[i][j]; - unsigned long matchesi = matchidi.size(); + std::vector & matchidi = matchids[i][j]; - Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); - const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); - const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); - const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); - if(type == PointToPlane){ - for(unsigned long ki = 0; ki < nr_api; ++ki) { - long kj = matchidi[ki]; - if( kj < 0 || kj >= nr_apj){continue;} - const double & src_x = api[3*ki+0]; - const double & src_y = api[3*ki+1]; - const double & src_z = api[3*ki+2]; + if(type == PointToPlane){ + for(unsigned long ki = 0; ki < nr_api; ++ki) { + long kj = matchidi[ki]; + if( kj < 0 || kj >= nr_apj){continue;} + const double & src_x = api[3*ki+0]; + const double & src_y = api[3*ki+1]; + const double & src_z = api[3*ki+2]; - const double & src_nx = ani[3*ki+0]; - const double & src_ny = ani[3*ki+1]; - const double & src_nz = ani[3*ki+2]; + const double & src_nx = ani[3*ki+0]; + const double & src_ny = ani[3*ki+1]; + const double & src_nz = ani[3*ki+2]; - const double & info_i = aii[ki]; + const double & info_i = aii[ki]; - const double & dst_x = apj[3*kj+0]; - const double & dst_y = apj[3*kj+1]; - const double & dst_z = apj[3*kj+2]; + const double & dst_x = apj[3*kj+0]; + const double & dst_y = apj[3*kj+1]; + const double & dst_z = apj[3*kj+2]; - const double & info_j = aij[kj]; + const double & info_j = aij[kj]; - float tx = m00*src_x + m01*src_y + m02*src_z + m03; - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float tx = m00*src_x + m01*src_y + m02*src_z + m03; + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; - float txn = m00*src_nx + m01*src_ny + m02*src_nz; - float tyn = m10*src_nx + m11*src_ny + m12*src_nz; - float tzn = m20*src_nx + m21*src_ny + m22*src_nz; + float txn = m00*src_nx + m01*src_ny + m02*src_nz; + float tyn = m10*src_nx + m11*src_ny + m12*src_nz; + float tzn = m20*src_nx + m21*src_ny + m22*src_nz; - const double rw = 1.0/(1.0/info_i+1.0/info_j); - const double di = (txn*(tx-dst_x) + tyn*(ty-dst_y) + tzn*(tz-dst_z))*rw; - all_residuals(0,count) = di; - count++; - } + const double rw = 1.0/(1.0/info_i+1.0/info_j); + const double di = (txn*(tx-dst_x) + tyn*(ty-dst_y) + tzn*(tz-dst_z))*rw; + all_residuals(0,count) = di; + count++; } + } - if(type == PointToPoint){ - for(unsigned long ki = 0; ki < nr_api; ++ki) { - long kj = matchidi[ki]; - if( kj < 0 ){continue;} - const double & src_x = api[3*ki+0]; - const double & src_y = api[3*ki+1]; - const double & src_z = api[3*ki+2]; + if(type == PointToPoint){ + for(unsigned long ki = 0; ki < nr_api; ++ki) { + long kj = matchidi[ki]; + if( kj < 0 ){continue;} + const double & src_x = api[3*ki+0]; + const double & src_y = api[3*ki+1]; + const double & src_z = api[3*ki+2]; - const double & info_i = aii[ki]; + const double & info_i = aii[ki]; - const double & dst_x = apj[3*kj+0]; - const double & dst_y = apj[3*kj+1]; - const double & dst_z = apj[3*kj+2]; + const double & dst_x = apj[3*kj+0]; + const double & dst_y = apj[3*kj+1]; + const double & dst_z = apj[3*kj+2]; - const double & info_j = aij[kj]; + const double & info_j = aij[kj]; - float tx = m00*src_x + m01*src_y + m02*src_z + m03;// const unsigned long src_nr_ap = kp_nr_arraypoints[i]; - // if(src_nr_ap == 0){continue;} - float ty = m10*src_x + m11*src_y + m12*src_z + m13; - float tz = m20*src_x + m21*src_y + m22*src_z + m23; + float tx = m00*src_x + m01*src_y + m02*src_z + m03;// const unsigned long src_nr_ap = kp_nr_arraypoints[i]; + // if(src_nr_ap == 0){continue;} + float ty = m10*src_x + m11*src_y + m12*src_z + m13; + float tz = m20*src_x + m21*src_y + m22*src_z + m23; - const double rw = 1.0/(1.0/info_i+1.0/info_j); - all_residuals(0,count) = (tx-dst_x)*rw; - all_residuals(1,count) = (ty-dst_y)*rw; - all_residuals(2,count) = (tz-dst_z)*rw; - count++; - } + const double rw = 1.0/(1.0/info_i+1.0/info_j); + all_residuals(0,count) = (tx-dst_x)*rw; + all_residuals(1,count) = (ty-dst_y)*rw; + all_residuals(2,count) = (tz-dst_z)*rw; + count++; } } } } - - if(all_residuals_type == 0){ - switch(type) { - case PointToPoint: {all_residuals = Eigen::Matrix3Xd::Zero(3,total_matches);}break; - case PointToPlane: {all_residuals = Eigen::MatrixXd::Zero(1,total_matches);}break; - default: {printf("type not set\n");} break; - } - - long count = 0; - for(unsigned long i = 0; i < nr_frames; i++){ - if(!is_ok[i]){continue;} - Eigen::Matrix & tXi = transformed_points[i]; - Eigen::Matrix & tXni = transformed_normals[i]; - Eigen::VectorXd & informationi = informations[i]; - for(unsigned long j = 0; j < nr_frames; j++){ - if(!is_ok[j]){continue;} - if(i == j){continue;} - std::vector & matchidi = matchids[i][j]; - unsigned long matchesi = matchidi.size(); - Eigen::Matrix & tXj = transformed_points[j]; - Eigen::Matrix & tXnj = transformed_normals[j]; - Eigen::VectorXd & informationj = informations[j]; - Eigen::Matrix3Xd Xp = Eigen::Matrix3Xd::Zero(3, matchesi); - Eigen::Matrix3Xd Xn = Eigen::Matrix3Xd::Zero(3, matchesi); - Eigen::Matrix3Xd Qp = Eigen::Matrix3Xd::Zero(3, matchesi); - Eigen::Matrix3Xd Qn = Eigen::Matrix3Xd::Zero(3, matchesi); - Eigen::VectorXd rangeW = Eigen::VectorXd::Zero( matchesi); - - for(unsigned long ki = 0; ki < matchesi; ki++){ - long kj = matchidi[ki]; - if( ki >= Qp.cols() || kj < 0 || kj >= tXj.cols() ){continue;} - Qp.col(ki) = tXj.col(kj); - Qn.col(ki) = tXnj.col(kj); - Xp.col(ki) = tXi.col(ki); - Xn.col(ki) = tXni.col(ki); - rangeW(ki) = 1.0/(1.0/informationi(ki)+1.0/informationj(kj)); - } - Eigen::MatrixXd residuals; - switch(type) { - case PointToPoint: {residuals = Xp-Qp;} break; - case PointToPlane: { - residuals = Eigen::MatrixXd::Zero(1, Xp.cols()); - for(long i=0; i >() ); - matchdists.push_back( std::vector< std::vector >() ); + matchdists.push_back( std::vector< std::vector >() ); nr_arraypoints.push_back(0); arraypoints.push_back(0); @@ -1305,11 +1236,11 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ bool * maskvec = mmask->maskvec; unsigned char * edgedata = (unsigned char *)(frame->depthedges.data); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - float * normalsdata = (float *)(frame->normals.data); + unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); + unsigned short * depthdata = (unsigned short *)(frame->depth.data); + float * normalsdata = (float *)(frame->normals.data); - Camera * camera = frame->camera; + Camera * camera = frame->camera; const unsigned long width = camera->width; const unsigned long height = camera->height; const float idepth = camera->idepth_scale; @@ -1322,7 +1253,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ for(unsigned long w = 0; w < width; w+=maskstep){ for(unsigned long h = 0; h < height; h+=maskstep){ long ind = h*width+w; - if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ + if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ float z = idepth*float(depthdata[ind]); float xn = normalsdata[3*ind+0]; if(z > 0.2 && xn != 2){count++;} @@ -1338,7 +1269,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ is_ok[i] = true; } -// printf("%i is ok %i\n",i,is_ok[i]); + // printf("%i is ok %i\n",i,is_ok[i]); nr_datas[i] = count; points[i].resize(Eigen::NoChange,count); @@ -1369,8 +1300,8 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ for(unsigned long h = 0; h < height;h+=maskstep){ if(c == count){continue;} long ind = h*width+w; - //if(maskvec[ind] || nomask){ - if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ + //if(maskvec[ind] || nomask){ + if(((w % maskstep == 0) && (h % maskstep == 0) && maskvec[ind]) || ( (w % nomaskstep == 0) && (h % nomaskstep == 0) && nomask)){ float z = idepth*float(depthdata[ind]); float xn = normalsdata[3*ind+0]; @@ -1466,25 +1397,25 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ depthedge_a3dv[i] = depthedge_a3d; depthedge_trees3d[i] = new Tree3d(3, *depthedge_a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); depthedge_trees3d[i]->buildIndex(); - Tree3d * t3d = depthedge_trees3d[i]; - - -// const long nrdn = depthedge_nr_neighbours+1; -// - -//#if defdomultithread -// #pragma omp parallel for num_threads(8) -//#endif -// for(long c = 0; c < depthedge_count; c++){ -// size_t ret_index[nrdn]; -// double out_dist_sqr[nrdn]; -// nanoflann::KNNResultSet resultSet(nrdn); -// resultSet.init(ret_index, out_dist_sqr ); -// t3d->findNeighbors(resultSet, depthedge_ap+3*c, nanoflann::SearchParams(10)); -// for(long k = 0; k < depthedge_nr_neighbours; k++){ -// dn[depthedge_nr_neighbours*c + k] = ret_index[k+1]; -// } -// } + Tree3d * t3d = depthedge_trees3d[i]; + + + // const long nrdn = depthedge_nr_neighbours+1; + // + + //#if defdomultithread + // #pragma omp parallel for num_threads(8) + //#endif + // for(long c = 0; c < depthedge_count; c++){ + // size_t ret_index[nrdn]; + // double out_dist_sqr[nrdn]; + // nanoflann::KNNResultSet resultSet(nrdn); + // resultSet.init(ret_index, out_dist_sqr ); + // t3d->findNeighbors(resultSet, depthedge_ap+3*c, nanoflann::SearchParams(10)); + // for(long k = 0; k < depthedge_nr_neighbours; k++){ + // dn[depthedge_nr_neighbours*c + k] = ret_index[k+1]; + // } + // } } //printf("total load time: %5.5f\n",getTime()-total_load_time_start); @@ -1493,9 +1424,9 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ void MassRegistrationPPR2::addData(pcl::PointCloud::Ptr cloud){ double total_load_time_start = getTime(); - nr_matches.push_back( 0); + nr_matches.push_back( 0); matchids.push_back( std::vector< std::vector >() ); - matchdists.push_back( std::vector< std::vector >() ); + matchdists.push_back( std::vector< std::vector >() ); nr_datas.push_back( 0); points.push_back( Eigen::Matrix()); @@ -1607,11 +1538,11 @@ void MassRegistrationPPR2::addData(pcl::PointCloud::Ptr informations[i] = information; - kp_nr_arraypoints.push_back(0); - kp_arraypoints.push_back(0); - kp_arraynormals.push_back(0); - kp_arrayinformations.push_back(0); - kp_arraydescriptors.push_back(0); + kp_nr_arraypoints.push_back(0); + kp_arraypoints.push_back(0); + kp_arraynormals.push_back(0); + kp_arrayinformations.push_back(0); + kp_arraydescriptors.push_back(0); ArrayData3D * a3d = new ArrayData3D; a3d->data = ap; @@ -1619,9 +1550,9 @@ void MassRegistrationPPR2::addData(pcl::PointCloud::Ptr a3dv[i] = a3d; trees3d[i] = new Tree3d(3, *a3d, nanoflann::KDTreeSingleIndexAdaptorParams(10)); trees3d[i]->buildIndex(); -// if(visualizationLvl > 0){ -// printf("total load time: %5.5f\n",getTime()-total_load_time_start); -// } + // if(visualizationLvl > 0){ + // printf("total load time: %5.5f\n",getTime()-total_load_time_start); + // } printf("total load time: %5.5f\n",getTime()-total_load_time_start); } @@ -1650,10 +1581,10 @@ std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vector 0 && kp_matches[i].size() > 0 && kp_matches[i][j].size() > 0){ - std::vector< TestMatch > & matches = kp_matches[i][j]; + /* + if(kp_matches.size() > 0 && kp_matches[i].size() > 0 && kp_matches[i][j].size() > 0){ + std::vector< TestMatch > & matches = kp_matches[i][j]; unsigned long nr_matches = matches.size(); for(unsigned long k = 0; k < nr_matches; k++){ - TestMatch & m = matches[k]; + TestMatch & m = matches[k]; long ki = m.src; long kj = m.dst; - kp_Xp_arr[3*kp_count+0] = kp_api[3*ki+0]; - kp_Xp_arr[3*kp_count+1] = kp_api[3*ki+1]; - kp_Xp_arr[3*kp_count+2] = kp_api[3*ki+2]; + kp_Xp_arr[3*kp_count+0] = kp_api[3*ki+0]; + kp_Xp_arr[3*kp_count+1] = kp_api[3*ki+1]; + kp_Xp_arr[3*kp_count+2] = kp_api[3*ki+2]; - kp_Xn_arr[3*kp_count+0] = kp_ani[3*ki+0]; - kp_Xn_arr[3*kp_count+1] = kp_ani[3*ki+1]; - kp_Xn_arr[3*kp_count+2] = kp_ani[3*ki+2]; - const double & info_i = kp_aii[ki]; + kp_Xn_arr[3*kp_count+0] = kp_ani[3*ki+0]; + kp_Xn_arr[3*kp_count+1] = kp_ani[3*ki+1]; + kp_Xn_arr[3*kp_count+2] = kp_ani[3*ki+2]; + const double & info_i = kp_aii[ki]; - const double & dst_x = kp_apj[3*kj+0]; - const double & dst_y = kp_apj[3*kj+1]; - const double & dst_z = kp_apj[3*kj+2]; + const double & dst_x = kp_apj[3*kj+0]; + const double & dst_y = kp_apj[3*kj+1]; + const double & dst_z = kp_apj[3*kj+2]; - const double & dst_nx = kp_anj[3*kj+0]; - const double & dst_ny = kp_anj[3*kj+1]; - const double & dst_nz = kp_anj[3*kj+2]; + const double & dst_nx = kp_anj[3*kj+0]; + const double & dst_ny = kp_anj[3*kj+1]; + const double & dst_nz = kp_anj[3*kj+2]; - const double & info_j = kp_aij[kj]; + const double & info_j = kp_aij[kj]; - kp_Qp_arr[3*kp_count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03; - kp_Qp_arr[3*kp_count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13; - kp_Qp_arr[3*kp_count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23; + kp_Qp_arr[3*kp_count+0] = mj00*dst_x + mj01*dst_y + mj02*dst_z + mj03; + kp_Qp_arr[3*kp_count+1] = mj10*dst_x + mj11*dst_y + mj12*dst_z + mj13; + kp_Qp_arr[3*kp_count+2] = mj20*dst_x + mj21*dst_y + mj22*dst_z + mj23; - kp_Qn_arr[3*kp_count+0] = mj00*dst_nx + mj01*dst_ny + mj02*dst_nz; - kp_Qn_arr[3*kp_count+1] = mj10*dst_nx + mj11*dst_ny + mj12*dst_nz; - kp_Qn_arr[3*kp_count+2] = mj20*dst_nx + mj21*dst_ny + mj22*dst_nz; + kp_Qn_arr[3*kp_count+0] = mj00*dst_nx + mj01*dst_ny + mj02*dst_nz; + kp_Qn_arr[3*kp_count+1] = mj10*dst_nx + mj11*dst_ny + mj12*dst_nz; + kp_Qn_arr[3*kp_count+2] = mj20*dst_nx + mj21*dst_ny + mj22*dst_nz; - kp_rangeW_arr[kp_count] = 1.0/(1.0/info_i+1.0/info_j); - kp_count++; - } - } + kp_rangeW_arr[kp_count] = 1.0/(1.0/info_i+1.0/info_j); + kp_count++; + } + } */ } @@ -1954,11 +1885,11 @@ std::vector MassRegistrationPPR2::optimize(std::vector MassRegistrationPPR2::optimize(std::vectorgetProb(rw*diffZ); double prob = probX*probY*probZ/(probX*probY*probZ + (1-probX)*(1-probY)*(1-probZ)); -//printf("%5.5i -> %5.5f %5.5f %5.5f -> %5.5f %5.5f %5.5f , %5.5f %5.5f %5.5f -> prob: %5.5f\n",co,src_x,src_y,src_z,sx,sy,sz,dx,dy,dz,prob); + //printf("%5.5i -> %5.5f %5.5f %5.5f -> %5.5f %5.5f %5.5f , %5.5f %5.5f %5.5f -> prob: %5.5f\n",co,src_x,src_y,src_z,sx,sy,sz,dx,dy,dz,prob); if(prob < 0.000001){continue;} -// if(visualizationLvl == 5){ -// pcl::PointXYZRGBNormal p; -// p.x = sx; -// p.y = sy; -// p.z = sz; -// p.b = 0; -// p.g = 255; -// p.r = 0; - -// pcl::PointXYZRGBNormal p1; -// p1.x = dx; -// p1.y = dy; -// p1.z = dz; -// p1.b = 255.0; -// p1.g = 0; -// p1.r = 0; -// dcloud->points.push_back(p1); - -//// char buf [1024]; -//// sprintf(buf,"line%i",scloud->points.size()); -//// viewer->addLine(p,p1,1.0-prob,prob,0,buf); -// } + // if(visualizationLvl == 5){ + // pcl::PointXYZRGBNormal p; + // p.x = sx; + // p.y = sy; + // p.z = sz; + // p.b = 0; + // p.g = 255; + // p.r = 0; + + // pcl::PointXYZRGBNormal p1; + // p1.x = dx; + // p1.y = dy; + // p1.z = dz; + // p1.b = 255.0; + // p1.g = 0; + // p1.r = 0; + // dcloud->points.push_back(p1); + + //// char buf [1024]; + //// sprintf(buf,"line%i",scloud->points.size()); + //// viewer->addLine(p,p1,1.0-prob,prob,0,buf); + // } if(visualizationLvl == 5){ -// printf("%5.5i -> %5.5f %5.5f %5.5f , %5.5f %5.5f %5.5f \n",co,sx,sy,sz,dx,dy,dz); + // printf("%5.5i -> %5.5f %5.5f %5.5f , %5.5f %5.5f %5.5f \n",co,sx,sy,sz,dx,dy,dz); pcl::PointXYZRGBNormal p; p.x = sx; @@ -2108,7 +2039,7 @@ std::vector MassRegistrationPPR2::optimize(std::vectorgetProb(rw*diffZ); double prob = probX*probY*probZ/(probX*probY*probZ + (1-probX)*(1-probY)*(1-probZ)); - if(prob < 0.000001){continue;} + if(prob < 0.000001){continue;} if(visualizationLvl == 5){ pcl::PointXYZRGBNormal p; @@ -2150,15 +2081,15 @@ std::vector MassRegistrationPPR2::optimize(std::vectoraddLine(p,p1,1.0-prob,prob,0,buf); //viewer->addLine(p,p1, angle <= 0 , angle > 0 ,0,buf); -// sprintf(buf,"slineN%i",scloud->points.size()); -// viewer->addLine(p1,p1n,1,0,1,buf); + // sprintf(buf,"slineN%i",scloud->points.size()); + // viewer->addLine(p1,p1n,1,0,1,buf); -// sprintf(buf,"dlineN%i",scloud->points.size()); -// viewer->addLine(p,pn,1,1,0,buf); + // sprintf(buf,"dlineN%i",scloud->points.size()); + // viewer->addLine(p,pn,1,1,0,buf); } - //double weight = kpInfo*prob*rw*rw; - double weight = prob*rw*rw; + //double weight = kpInfo*prob*rw*rw; + double weight = prob*rw*rw; wsum += weight; wsx += weight * sx; @@ -2215,8 +2146,8 @@ std::vector MassRegistrationPPR2::optimize(std::vector (ATA.inverse () * ATb); @@ -2280,8 +2211,8 @@ std::vector MassRegistrationPPR2::optimize(std::vector & matchidi = matchids[i][j]; unsigned long matchesi = matchidi.size(); - std::vector & matchdistj = matchdists[j][i]; - std::vector & matchdisti = matchdists[i][j]; + std::vector & matchdistj = matchdists[j][i]; + std::vector & matchdisti = matchdists[i][j]; for(unsigned long ki = 0; ki < matchesi; ki++){ @@ -2446,8 +2377,8 @@ std::vector MassRegistrationPPR2::optimize(std::vector & matchdistj = matchdists[j][i]; - std::vector & matchdisti = matchdists[i][j]; + std::vector & matchdistj = matchdists[j][i]; + std::vector & matchdisti = matchdists[i][j]; for(unsigned long ki = 0; ki < matchesi; ki++){ @@ -2487,8 +2418,8 @@ std::vector MassRegistrationPPR2::optimize(std::vector & matchdistj = matchdists[j][i]; - std::vector & matchdisti = matchdists[i][j]; + std::vector & matchdistj = matchdists[j][i]; + std::vector & matchdisti = matchdists[i][j]; for(unsigned long ki = 0; ki < matchesi; ki++){ long kj = matchidi[ki]; @@ -2606,7 +2537,7 @@ std::vector MassRegistrationPPR2::optimize(std::vector poses, long max_kps = 0; for(unsigned long i = 0; i < nr_frames; i++){ - printf("max_kps: %i kp_nr_arraypoints: %i\n",max_kps, kp_nr_arraypoints[i]); - max_kps = std::max(max_kps,kp_nr_arraypoints[i]); - } + printf("max_kps: %i kp_nr_arraypoints: %i\n",max_kps, kp_nr_arraypoints[i]); + max_kps = std::max(max_kps,kp_nr_arraypoints[i]); + } printf("max_kps: %i\n",max_kps); - if(max_kps == 0){return;} + if(max_kps == 0){return;} double * best_src = new double[max_kps]; - double * best_src2 = new double[max_kps]; + double * best_src2 = new double[max_kps]; double * best_src_e = new double[max_kps]; double * best_src_f = new double[max_kps]; long * best_src_id = new long[max_kps]; @@ -2694,322 +2625,319 @@ void MassRegistrationPPR2::rematchKeyPoints(std::vector poses, long ignores = 0; -bool useOrb = true; -if(useOrb){ - for(unsigned long i = 0; i < nr_frames; i++){ - const unsigned long src_nr_ap = kp_nr_arraypoints[i]; - if(src_nr_ap == 0){continue;} - double * src_ap = kp_arraypoints[i]; - double * src_an = kp_arraynormals[i]; - double * src_ai = kp_arrayinformations[i]; - uint64_t * src_data = kp_arraydescriptors[i]; + bool useOrb = true; + if(useOrb){ + for(unsigned long i = 0; i < nr_frames; i++){ + const unsigned long src_nr_ap = kp_nr_arraypoints[i]; + if(src_nr_ap == 0){continue;} + double * src_ap = kp_arraypoints[i]; + double * src_an = kp_arraynormals[i]; + double * src_ai = kp_arrayinformations[i]; + uint64_t * src_data = kp_arraydescriptors[i]; - for(unsigned long j = 0; j < nr_frames; j++){ - if(i == j){continue;} + for(unsigned long j = 0; j < nr_frames; j++){ + if(i == j){continue;} - const unsigned long dst_nr_ap = kp_nr_arraypoints[j]; - if(dst_nr_ap == 0){continue;} + const unsigned long dst_nr_ap = kp_nr_arraypoints[j]; + if(dst_nr_ap == 0){continue;} - for(long k = 0; k < src_nr_ap; k++){ - best_src[k] = 999999999999; - best_src2[k] = 999999999999; - best_src_id[k] = -1; - } - for(long k = 0; k < dst_nr_ap; k++){ - best_dst[k] = 999999999999; - best_dst_id[k] = -1; - } + for(long k = 0; k < src_nr_ap; k++){ + best_src[k] = 999999999999; + best_src2[k] = 999999999999; + best_src_id[k] = -1; + } + for(long k = 0; k < dst_nr_ap; k++){ + best_dst[k] = 999999999999; + best_dst_id[k] = -1; + } - double * dst_ap = kp_arraypoints[j]; - double * dst_an = kp_arraynormals[j]; - double * dst_ai = kp_arrayinformations[j]; - uint64_t * dst_data = kp_arraydescriptors[j]; + double * dst_ap = kp_arraypoints[j]; + double * dst_an = kp_arraynormals[j]; + double * dst_ai = kp_arrayinformations[j]; + uint64_t * dst_data = kp_arraydescriptors[j]; - Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); - if(!first){ - Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); + if(!first){ + Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); - Eigen::Affine3d diff = prev_rp.inverse()*rp; + Eigen::Affine3d diff = prev_rp.inverse()*rp; - double change_trans = 0; - double change_rot = 0; - double dt = 0; - for(unsigned long k = 0; k < 3; k++){ - dt += diff(k,3)*diff(k,3); - for(unsigned long l = 0; l < 3; l++){ - if(k == l){ change_rot += fabs(1-diff(k,l));} - else{ change_rot += fabs(diff(k,l));} + double change_trans = 0; + double change_rot = 0; + double dt = 0; + for(unsigned long k = 0; k < 3; k++){ + dt += diff(k,3)*diff(k,3); + for(unsigned long l = 0; l < 3; l++){ + if(k == l){ change_rot += fabs(1-diff(k,l));} + else{ change_rot += fabs(diff(k,l));} + } } - } - change_trans += sqrt(dt); + change_trans += sqrt(dt); - if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;} - } + if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;} + } - const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); - const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); - const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); - float di = 10.0; - float pi = 10.0; + float di = 10.0; + float pi = 10.0; #if defdomultithread - #pragma omp parallel for num_threads(8) +#pragma omp parallel for num_threads(8) #endif - for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { - const double & src_x = src_ap[3*src_k+0]; - const double & src_y = src_ap[3*src_k+1]; - const double & src_z = src_ap[3*src_k+2]; - - double sx = m00*src_x + m01*src_y + m02*src_z + m03; - double sy = m10*src_x + m11*src_y + m12*src_z + m13; - double sz = m20*src_x + m21*src_y + m22*src_z + m23; - - unsigned long src_k4 = src_k*4; - const uint64_t s1 = src_data[src_k4+0]; - const uint64_t s2 = src_data[src_k4+1]; - const uint64_t s3 = src_data[src_k4+2]; - const uint64_t s4 = src_data[src_k4+3]; - - uint64_t xordata [4]; - for(unsigned long dst_k = 0; dst_k < dst_nr_ap; ++dst_k) { - double dx = sx-dst_ap[3*dst_k+0]; - double dy = sy-dst_ap[3*dst_k+1]; - double dz = sz-dst_ap[3*dst_k+2]; - - double p_dist = sqrt(dx*dx+dy*dy+dz*dz); - - unsigned long dst_k4 = dst_k*4; - const uint64_t d1 = dst_data[dst_k4+0]; - const uint64_t d2 = dst_data[dst_k4+1]; - const uint64_t d3 = dst_data[dst_k4+2]; - const uint64_t d4 = dst_data[dst_k4+3]; - - xordata[0] = s1 ^ d1; - xordata[1] = s2 ^ d2; - xordata[2] = s3 ^ d3; - xordata[3] = s4 ^ d4; - - long cnt = popcount_lauradoux(xordata, 4); - double f_dist = float(cnt)/256.0f; - - float d = f_dist*di + p_dist*pi; - - if(d < best_src[src_k]){ + for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { + const double & src_x = src_ap[3*src_k+0]; + const double & src_y = src_ap[3*src_k+1]; + const double & src_z = src_ap[3*src_k+2]; + + double sx = m00*src_x + m01*src_y + m02*src_z + m03; + double sy = m10*src_x + m11*src_y + m12*src_z + m13; + double sz = m20*src_x + m21*src_y + m22*src_z + m23; + + unsigned long src_k4 = src_k*4; + const uint64_t s1 = src_data[src_k4+0]; + const uint64_t s2 = src_data[src_k4+1]; + const uint64_t s3 = src_data[src_k4+2]; + const uint64_t s4 = src_data[src_k4+3]; + + uint64_t xordata [4]; + for(unsigned long dst_k = 0; dst_k < dst_nr_ap; ++dst_k) { + double dx = sx-dst_ap[3*dst_k+0]; + double dy = sy-dst_ap[3*dst_k+1]; + double dz = sz-dst_ap[3*dst_k+2]; + + double p_dist = sqrt(dx*dx+dy*dy+dz*dz); + + unsigned long dst_k4 = dst_k*4; + const uint64_t d1 = dst_data[dst_k4+0]; + const uint64_t d2 = dst_data[dst_k4+1]; + const uint64_t d3 = dst_data[dst_k4+2]; + const uint64_t d4 = dst_data[dst_k4+3]; + + xordata[0] = s1 ^ d1; + xordata[1] = s2 ^ d2; + xordata[2] = s3 ^ d3; + xordata[3] = s4 ^ d4; + + long cnt = popcount_lauradoux(xordata, 4); + double f_dist = float(cnt)/256.0f; + + float d = f_dist*di + p_dist*pi; + + if(d < best_src[src_k]){ + + best_src2[src_k] = best_src[src_k]; + best_src_id[src_k] = dst_k; + best_src[src_k] = d; + }else if(d < best_src2[src_k]){ + best_src2[src_k] = d; + } - best_src2[src_k] = best_src[src_k]; - best_src_id[src_k] = dst_k; - best_src[src_k] = d; - }else if(d < best_src2[src_k]){ - best_src2[src_k] = d; - } + if(d < best_dst[dst_k]){ + best_dst_id[dst_k] = src_k; + best_dst[dst_k] = d; + } - if(d < best_dst[dst_k]){ - best_dst_id[dst_k] = src_k; - best_dst[dst_k] = d; } - } - } - std::vector< TestMatch > matches; - double threshold = 2.0; - double ratiothreshold = 0.8; + std::vector< TestMatch > matches; + double threshold = 2.0; + double ratiothreshold = 0.8; - long nr_matches = 0; - for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { - long dst_k = best_src_id[src_k]; + long nr_matches = 0; + for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { + long dst_k = best_src_id[src_k]; - if(best_dst_id[dst_k] != src_k){continue;}//One to one - if(best_src[src_k] > threshold){continue;}//threshold - if(best_src[src_k]/best_src2[src_k] < ratiothreshold){continue;}//ratiothreshold + if(best_dst_id[dst_k] != src_k){continue;}//One to one + if(best_src[src_k] > threshold){continue;}//threshold + if(best_src[src_k]/best_src2[src_k] < ratiothreshold){continue;}//ratiothreshold - if(dst_an[3*dst_k+0] > 1 || src_an[3*src_k+0] > 1){continue;} + if(dst_an[3*dst_k+0] > 1 || src_an[3*src_k+0] > 1){continue;} - nr_matches++; + nr_matches++; - matches.push_back(TestMatch(src_k, dst_k,best_src[src_k])); + matches.push_back(TestMatch(src_k, dst_k,best_src[src_k])); + } + kp_matches[i][j] = matches; } - kp_matches[i][j] = matches; } } -} -bool useSurf = false; -if(useSurf){ - for(unsigned long i = 0; i < nr_frames; i++){ - const unsigned long src_nr_ap = kp_nr_arraypoints[i]; - if(src_nr_ap == 0){continue;} - double * src_ap = kp_arraypoints[i]; - double * src_an = kp_arraynormals[i]; - double * src_ai = kp_arrayinformations[i]; - float* src_data = (float*)(kp_arraydescriptors[i]); + bool useSurf = false; + if(useSurf){ + for(unsigned long i = 0; i < nr_frames; i++){ + const unsigned long src_nr_ap = kp_nr_arraypoints[i]; + if(src_nr_ap == 0){continue;} + double * src_ap = kp_arraypoints[i]; + double * src_an = kp_arraynormals[i]; + double * src_ai = kp_arrayinformations[i]; + float* src_data = (float*)(kp_arraydescriptors[i]); - for(unsigned long j = 0; j < nr_frames; j++){ - if(i == j){continue;} + for(unsigned long j = 0; j < nr_frames; j++){ + if(i == j){continue;} - const unsigned long dst_nr_ap = kp_nr_arraypoints[j]; - if(dst_nr_ap == 0){continue;} + const unsigned long dst_nr_ap = kp_nr_arraypoints[j]; + if(dst_nr_ap == 0){continue;} - for(long k = 0; k < src_nr_ap; k++){ - best_src[k] = 999999999999; - best_src_id[k] = -1; - } - for(long k = 0; k < dst_nr_ap; k++){ - best_dst[k] = 999999999999; - best_dst_id[k] = -1; - } + for(long k = 0; k < src_nr_ap; k++){ + best_src[k] = 999999999999; + best_src_id[k] = -1; + } + for(long k = 0; k < dst_nr_ap; k++){ + best_dst[k] = 999999999999; + best_dst_id[k] = -1; + } - double * dst_ap = kp_arraypoints[j]; - double * dst_an = kp_arraynormals[j]; - double * dst_ai = kp_arrayinformations[j]; - float* dst_data = (float*)(kp_arraydescriptors[j]); + double * dst_ap = kp_arraypoints[j]; + double * dst_an = kp_arraynormals[j]; + double * dst_ai = kp_arrayinformations[j]; + float* dst_data = (float*)(kp_arraydescriptors[j]); - Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); + Eigen::Affine3d rp = Eigen::Affine3d(poses[j].inverse()*poses[i]); - if(!first){ - Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); + if(!first){ + Eigen::Affine3d prev_rp = Eigen::Affine3d(prev_poses[j].inverse()*prev_poses[i]); - Eigen::Affine3d diff = prev_rp.inverse()*rp; + Eigen::Affine3d diff = prev_rp.inverse()*rp; - double change_trans = 0; - double change_rot = 0; - double dt = 0; - for(unsigned long k = 0; k < 3; k++){ - dt += diff(k,3)*diff(k,3); - for(unsigned long l = 0; l < 3; l++){ - if(k == l){ change_rot += fabs(1-diff(k,l));} - else{ change_rot += fabs(diff(k,l));} - } - } - change_trans += sqrt(dt); + double change_trans = 0; + double change_rot = 0; + double dt = 0; + for(unsigned long k = 0; k < 3; k++){ + dt += diff(k,3)*diff(k,3); + for(unsigned long l = 0; l < 3; l++){ + if(k == l){ change_rot += fabs(1-diff(k,l));} + else{ change_rot += fabs(diff(k,l));} + } + } + change_trans += sqrt(dt); - if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;} - } + if(change_trans < 1*stopval && change_rot < 1*stopval){ignores++;continue;} + } - const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); - const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); - const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); + const double & m00 = rp(0,0); const double & m01 = rp(0,1); const double & m02 = rp(0,2); const double & m03 = rp(0,3); + const double & m10 = rp(1,0); const double & m11 = rp(1,1); const double & m12 = rp(1,2); const double & m13 = rp(1,3); + const double & m20 = rp(2,0); const double & m21 = rp(2,1); const double & m22 = rp(2,2); const double & m23 = rp(2,3); #if defdomultithread - #pragma omp parallel for num_threads(8) +#pragma omp parallel for num_threads(8) #endif - for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { - const double & src_x = src_ap[3*src_k+0]; - const double & src_y = src_ap[3*src_k+1]; - const double & src_z = src_ap[3*src_k+2]; + for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { + const double & src_x = src_ap[3*src_k+0]; + const double & src_y = src_ap[3*src_k+1]; + const double & src_z = src_ap[3*src_k+2]; - double sx = m00*src_x + m01*src_y + m02*src_z + m03; - double sy = m10*src_x + m11*src_y + m12*src_z + m13; - double sz = m20*src_x + m21*src_y + m22*src_z + m23; + double sx = m00*src_x + m01*src_y + m02*src_z + m03; + double sy = m10*src_x + m11*src_y + m12*src_z + m13; + double sz = m20*src_x + m21*src_y + m22*src_z + m23; - for(unsigned long dst_k = 0; dst_k < dst_nr_ap; ++dst_k) { - double dx = sx-dst_ap[3*dst_k+0]; - double dy = sy-dst_ap[3*dst_k+1]; - double dz = sz-dst_ap[3*dst_k+2]; + for(unsigned long dst_k = 0; dst_k < dst_nr_ap; ++dst_k) { + double dx = sx-dst_ap[3*dst_k+0]; + double dy = sy-dst_ap[3*dst_k+1]; + double dz = sz-dst_ap[3*dst_k+2]; - double f_dist = 0; + double f_dist = 0; - for(unsigned long f_k = 0; f_k < 128; ++f_k) { - double diff = src_data[src_k*128 + f_k] - dst_data[dst_k*128 + f_k]; - f_dist += diff*diff; - } + for(unsigned long f_k = 0; f_k < 128; ++f_k) { + double diff = src_data[src_k*128 + f_k] - dst_data[dst_k*128 + f_k]; + f_dist += diff*diff; + } - float d = f_dist;//*di + p_dist*pi; + float d = f_dist;//*di + p_dist*pi; - if(d < best_src[src_k]){ - best_src_id[src_k] = dst_k; - best_src[src_k] = d; - } + if(d < best_src[src_k]){ + best_src_id[src_k] = dst_k; + best_src[src_k] = d; + } - if(d < best_dst[dst_k]){ - best_dst_id[dst_k] = src_k; - best_dst[dst_k] = d; - } + if(d < best_dst[dst_k]){ + best_dst_id[dst_k] = src_k; + best_dst[dst_k] = d; + } - } - } + } + } - std::vector< TestMatch > matches; - double threshold = 0.25; - long nr_matches = 0; - for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { - long dst_k = best_src_id[src_k]; + std::vector< TestMatch > matches; + double threshold = 0.25; + long nr_matches = 0; + for(unsigned long src_k = 0; src_k < src_nr_ap; ++src_k) { + long dst_k = best_src_id[src_k]; - if(best_dst_id[dst_k] != src_k || best_src[src_k] > threshold){continue;}//One to one + if(best_dst_id[dst_k] != src_k || best_src[src_k] > threshold){continue;}//One to one - if(dst_an[3*dst_k+0] > 1 || src_an[3*src_k+0] > 1){continue;} + if(dst_an[3*dst_k+0] > 1 || src_an[3*src_k+0] > 1){continue;} - nr_matches++; + nr_matches++; - matches.push_back(TestMatch(src_k, dst_k,best_src[src_k])); - } - kp_matches[i][j] = matches; - } - } -} -if(false){ - for(unsigned long i = 0; i < nr_frames; i++){ - if(frameid[i] == -1){continue;} - for(unsigned long j = 0; j < nr_frames; j++){ - if(frameid[j] == -1){continue;} - if(kp_matches[i][j].size() == 0){continue;} - unsigned char * src_data = (unsigned char *)(model->frames[frameid[i]]->rgb.data); - unsigned char * dst_data = (unsigned char *)(model->frames[frameid[j]]->rgb.data); - - cv::Mat img; - img.create(480,2*640,CV_8UC3); - unsigned char * imgdata = (unsigned char *)img.data; - for(long w = 0; w < 640; w++){ - for(long h = 0; h < 480; h++){ - long ind = h*640+w; - - long ind2 = h*2*640+w; - long ind3 = h*2*640+w+640; - - imgdata[3*ind2+0] = src_data[3*ind+0]; - imgdata[3*ind2+1] = src_data[3*ind+1]; - imgdata[3*ind2+2] = src_data[3*ind+2]; - - imgdata[3*ind3+0] = dst_data[3*ind+0]; - imgdata[3*ind3+1] = dst_data[3*ind+1]; - imgdata[3*ind3+2] = dst_data[3*ind+2]; - } - } - - std::vector & src_keypoints = model->all_keypoints[i]; - std::vector & dst_keypoints = model->all_keypoints[j]; - - std::vector< TestMatch > matches = kp_matches[i][j]; - for(unsigned long k = 0; k < matches.size(); ++k) { - cv::KeyPoint & spt = src_keypoints[matches[k].src]; - cv::KeyPoint & dpt = dst_keypoints[matches[k].dst]; - - //printf("%i -> ") - - cv::line(img, spt.pt, cv::Point(dpt.pt.x+640,dpt.pt.y), cv::Scalar(255,0,255)); - } - - cv::imshow( "matches", img ); - cv::waitKey(0); - - //for(long j = 0; j < width*height; j++){maskdata[j] = 255*maskvec[j];} - } - } -} + matches.push_back(TestMatch(src_k, dst_k,best_src[src_k])); + } + kp_matches[i][j] = matches; + } + } + } + if(false){ + for(unsigned long i = 0; i < nr_frames; i++){ + if(frameid[i] == -1){continue;} + for(unsigned long j = 0; j < nr_frames; j++){ + if(frameid[j] == -1){continue;} + if(kp_matches[i][j].size() == 0){continue;} + unsigned char * src_data = (unsigned char *)(model->frames[frameid[i]]->rgb.data); + unsigned char * dst_data = (unsigned char *)(model->frames[frameid[j]]->rgb.data); + + cv::Mat img; + img.create(480,2*640,CV_8UC3); + unsigned char * imgdata = (unsigned char *)img.data; + for(long w = 0; w < 640; w++){ + for(long h = 0; h < 480; h++){ + long ind = h*640+w; + + long ind2 = h*2*640+w; + long ind3 = h*2*640+w+640; + + imgdata[3*ind2+0] = src_data[3*ind+0]; + imgdata[3*ind2+1] = src_data[3*ind+1]; + imgdata[3*ind2+2] = src_data[3*ind+2]; + + imgdata[3*ind3+0] = dst_data[3*ind+0]; + imgdata[3*ind3+1] = dst_data[3*ind+1]; + imgdata[3*ind3+2] = dst_data[3*ind+2]; + } + } + + std::vector & src_keypoints = model->all_keypoints[i]; + std::vector & dst_keypoints = model->all_keypoints[j]; + + std::vector< TestMatch > matches = kp_matches[i][j]; + for(unsigned long k = 0; k < matches.size(); ++k) { + cv::KeyPoint & spt = src_keypoints[matches[k].src]; + cv::KeyPoint & dpt = dst_keypoints[matches[k].dst]; + cv::line(img, spt.pt, cv::Point(dpt.pt.x+640,dpt.pt.y), cv::Scalar(255,0,255)); + } + + cv::imshow( "matches", img ); + cv::waitKey(0); + + //for(long j = 0; j < width*height; j++){maskdata[j] = 255*maskvec[j];} + } + } + } delete[] best_src; delete[] best_src_e; @@ -3025,10 +2953,10 @@ if(false){ } void MassRegistrationPPR2::showEdges(std::vector poses){ - printf("showEdges\n"); + printf("showEdges\n"); pcl::PointCloud::Ptr cloud (new pcl::PointCloud); for(unsigned long i = 0; i < poses.size(); i++){ - printf("%i/%i -> %i\n",i,poses.size(),depthedge_nr_arraypoints[i]); + printf("%i/%i -> %i\n",i,poses.size(),depthedge_nr_arraypoints[i]); Eigen::Matrix4d p = poses[i]; float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); @@ -3057,11 +2985,11 @@ void MassRegistrationPPR2::showEdges(std::vector poses){ } printf("showEdges %i -> %i\n",i,cloud->points.size()); } - if(cloud->points.size() > 0){ - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); - } + if(cloud->points.size() > 0){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + } } MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses){ @@ -3069,7 +2997,7 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses0 = poses; - //rematchKeyPoints(poses,poses0,first); for(long funcupdate=0; funcupdate < 100; ++funcupdate) { if(getTime()-total_time_start > timeout){break;} @@ -3215,18 +3140,12 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses2b = poses; double residuals_time_start = getTime(); - Eigen::MatrixXd all_residuals; - - if(use_surface){ - all_residuals = getAllResiduals(poses); - } + Eigen::MatrixXd all_residuals; Eigen::MatrixXd depthedge_all_residuals; - if(use_depthedge){ - depthedge_all_residuals = depthedge_getAllResiduals(poses); - } + if(use_surface){ all_residuals = getAllResiduals(poses);} + if(use_depthedge){ depthedge_all_residuals = depthedge_getAllResiduals(poses);} -// Eigen::MatrixXd all_KPresiduals = getAllKpResiduals(poses); residuals_time += getTime()-residuals_time_start; double computeModel_time_start = getTime(); @@ -3237,18 +3156,18 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectorregularization,func->noiseval); //printf("edge: reg: %5.5f noise: %5.5f\n",depthedge_func->regularization,depthedge_func->noiseval); -// kpfunc->computeModel(all_KPresiduals); + // kpfunc->computeModel(all_KPresiduals); //stopval = func->getNoise()*0.1; -// printf("reg: %f noise:%f stopval: %f\n",func->regularization,func->noiseval,stopval); + // printf("reg: %f noise:%f stopval: %f\n",func->regularization,func->noiseval,stopval); -// double stopval1 = func->getNoise()*0.1; -// double stopval2 = kpfunc->getNoise()*0.1; -// if(func->noiseval > 0 && kpfunc->noiseval > 0){ stopval = std::min(stopval1,stopval2);} -// else if(func->noiseval > 0){ stopval = func->getNoise()*0.1;} -// else if(kpfunc->noiseval > 0){ stopval = kpfunc->getNoise()*0.1;} -// else{ break;} -// printf("reg: %f noise:%f stopval: %f\n",func->regularization,func->noiseval,stopval); -// printf("kpreg: %f kpnoise:%f stopval: %f\n",kpfunc->regularization,kpfunc->noiseval,stopval); + // double stopval1 = func->getNoise()*0.1; + // double stopval2 = kpfunc->getNoise()*0.1; + // if(func->noiseval > 0 && kpfunc->noiseval > 0){ stopval = std::min(stopval1,stopval2);} + // else if(func->noiseval > 0){ stopval = func->getNoise()*0.1;} + // else if(kpfunc->noiseval > 0){ stopval = kpfunc->getNoise()*0.1;} + // else{ break;} + // printf("reg: %f noise:%f stopval: %f\n",func->regularization,func->noiseval,stopval); + // printf("kpreg: %f kpnoise:%f stopval: %f\n",kpfunc->regularization,kpfunc->noiseval,stopval); computeModel_time += getTime()-computeModel_time_start; @@ -3258,7 +3177,7 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectorgetNoise(); @@ -3274,30 +3193,30 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectorgetNoise(); kpfunc->update(); double kpnoise_after = kpfunc->getNoise(); - double kpratio = kpnoise_after/kpnoise_before; + double kpratio = kpnoise_after/kpnoise_before; - double reg1 = func->regularization; - double reg2 = kpfunc->regularization; - func->regularization = std::max(reg1,reg2); - kpfunc->regularization = std::max(reg1,reg2); + double reg1 = func->regularization; + double reg2 = kpfunc->regularization; + func->regularization = std::max(reg1,reg2); + kpfunc->regularization = std::max(reg1,reg2); -// printf("func->noiseval: %f func->regularization: %f\n",func->noiseval,func->regularization); + // printf("func->noiseval: %f func->regularization: %f\n",func->noiseval,func->regularization); //printf("ratio: %f kpratio: %f\n",ratio,kpratio); - //if(func->noiseval >= kpfunc->noiseval && 0.01*func->noiseval < func->regularization){break;} - //if(func->noiseval < kpfunc->noiseval && 0.01*kpfunc->noiseval < kpfunc->regularization){break;} - //if(ratio > 0.99){break;} + //if(func->noiseval >= kpfunc->noiseval && 0.01*func->noiseval < func->regularization){break;} + //if(func->noiseval < kpfunc->noiseval && 0.01*kpfunc->noiseval < kpfunc->regularization){break;} + //if(ratio > 0.99){break;} if(func->noiseval > 10.0*func->regularization && ratio > 0.75){break;} } -// printf("total_time: %5.5f\n",getTime()-total_time_start); -// printf("rematch_time: %5.5f\n",rematch_time); -// printf("compM residuals_time:%5.5f\n",residuals_time); -// printf("computeModel: %5.5f\n",computeModel_time); -// printf("opt_time: %5.5f\n",opt_time); + // printf("total_time: %5.5f\n",getTime()-total_time_start); + // printf("rematch_time: %5.5f\n",rematch_time); + // printf("compM residuals_time:%5.5f\n",residuals_time); + // printf("computeModel: %5.5f\n",computeModel_time); + // printf("opt_time: %5.5f\n",opt_time); -// printf("setup_matches_time: %5.5f\n",setup_matches_time); -// printf("setup_equation_time: %5.5f\n",setup_equation_time); -// printf("solve_equation_time: %5.5f\n",solve_equation_time); + // printf("setup_matches_time: %5.5f\n",setup_matches_time); + // printf("setup_equation_time: %5.5f\n",setup_equation_time); + // printf("solve_equation_time: %5.5f\n",solve_equation_time); if(visualizationLvl > 0){ showEdges(poses); @@ -3311,8 +3230,6 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector guess)\n"); - //exit(0); return MassFusionResults(poses,-1); } From e30215d615b74453a68a3108b09b768cbb97fbee Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 18 Oct 2016 16:21:44 +0200 Subject: [PATCH 167/255] ... --- quasimodo/quasimodo_brain/CMakeLists.txt | 5 +- .../quasimodo_brain/src/AnnotationTool.cpp | 456 +++++++++++++ .../metaroom_additional_view_processing.cpp | 79 ++- .../quasimodo_models/include/core/Util.h | 2 + .../include/modelupdater/ModelUpdater.h | 8 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 176 +++-- quasimodo/quasimodo_models/src/core/Util.cpp | 12 + .../src/modelupdater/ModelUpdater.cpp | 605 +++++++++++++++++- 8 files changed, 1177 insertions(+), 166 deletions(-) create mode 100644 quasimodo/quasimodo_brain/src/AnnotationTool.cpp diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index e6800e1b..6f39fdf4 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -134,11 +134,14 @@ add_executable( segmentationserver src/segmentationserver.cpp) add_dependencies( segmentationserver roscpp quasimodo_msgs_generate_messages_cpp) target_link_libraries( segmentationserver quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) - add_executable( metaroom_additional_view_processing src/metaroom_additional_view_processing.cpp) add_dependencies( metaroom_additional_view_processing roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp) target_link_libraries( metaroom_additional_view_processing quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +add_executable( AnnotationTool src/AnnotationTool.cpp) +add_dependencies( AnnotationTool roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp) +target_link_libraries( AnnotationTool quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + ############# ## Install ## diff --git a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp new file mode 100644 index 00000000..748e512a --- /dev/null +++ b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp @@ -0,0 +1,456 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include +#include + +#include +#include + +#include "eigen_conversions/eigen_msg.h" +#include "tf_conversions/tf_eigen.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" +#include "metaroom_xml_parser/simple_summary_parser.h" + +#include + +#include "ros/ros.h" +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" +#include "quasimodo_msgs/segment_model.h" +#include "quasimodo_msgs/metaroom_pair.h" + +#include "ros/ros.h" +#include +#include +#include +#include +#include + +#include "metaroom_xml_parser/load_utilities.h" +#include +#include + +#include "quasimodo_msgs/model.h" +#include "quasimodo_msgs/rgbd_frame.h" +#include "quasimodo_msgs/model_from_frame.h" +#include "quasimodo_msgs/index_frame.h" +#include "quasimodo_msgs/fuse_models.h" +#include "quasimodo_msgs/get_model.h" + +#include "metaroom_xml_parser/simple_xml_parser.h" +#include "metaroom_xml_parser/load_utilities.h" + +#include +#include + +#include "modelupdater/ModelUpdater.h" +#include "core/RGBDFrame.h" +#include "Util/Util.h" + +// Services +#include +#include +#include + +// Registration service +#include + +// Additional view mask service +#include + + +#include +#include + +// Custom messages +#include "object_manager/dynamic_object.h" +#include "object_manager/dynamic_object_xml_parser.h" +#include "object_manager/dynamic_object_utilities.h" +#include "object_manager/dynamic_object_mongodb_interface.h" + +#include +#include + + +#include + + +std::vector< ros::ServiceServer > m_DynamicObjectsServiceServers; +std::vector< ros::ServiceServer > m_GetDynamicObjectServiceServers; + +typedef typename object_manager_msgs::DynamicObjectsService::Request DynamicObjectsServiceRequest; +typedef typename object_manager_msgs::DynamicObjectsService::Response DynamicObjectsServiceResponse; + +typedef typename object_manager_msgs::GetDynamicObjectService::Request GetDynamicObjectServiceRequest; +typedef typename object_manager_msgs::GetDynamicObjectService::Response GetDynamicObjectServiceResponse; + +typedef typename object_manager_msgs::ProcessDynamicObjectService::Request ProcessDynamicObjectServiceRequest; +typedef typename object_manager_msgs::ProcessDynamicObjectService::Response ProcessDynamicObjectServiceResponse; + +ros::ServiceClient segmentation_client; +using namespace std; +using namespace semantic_map_load_utilties; + +typedef pcl::PointXYZRGB PointType; +typedef pcl::PointCloud Cloud; +typedef typename Cloud::Ptr CloudPtr; +typedef pcl::search::KdTree Tree; +typedef semantic_map_load_utilties::DynamicObjectData ObjectData; + +using namespace std; +using namespace semantic_map_load_utilties; + +boost::shared_ptr viewer; +int visualization_lvl = 0; + +Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader){ + QXmlStreamReader::TokenType token = xmlReader->readNext();//Translation + QString elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//fx + double tx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//fy + double ty = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cx + double tz = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Translation + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//Rotation + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//qw + double qw = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qx + double qx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qy + double qy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qz + double qz = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Rotation + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d regpose = (Eigen::Affine3d(Eigen::Quaterniond(qw,qx,qy,qz))).matrix(); + regpose(0,3) = tx; + regpose(1,3) = ty; + regpose(2,3) = tz; + + return regpose; +} + +void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses){ + + QFile file(xmlFile.c_str()); + + if (!file.exists()) + { + ROS_ERROR("Could not open file %s to load room.",xmlFile.c_str()); + return; + } + + QString xmlFileQS(xmlFile.c_str()); + int index = xmlFileQS.lastIndexOf('/'); + std::string roomFolder = xmlFileQS.left(index).toStdString(); + + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()) + { + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()) + { + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + return; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement) + { + + if (xmlReader->name() == "View") + { + cv::Mat rgb; + cv::Mat depth; + //printf("elementName: %s\n",elementName.toStdString().c_str()); + QXmlStreamAttributes attributes = xmlReader->attributes(); + if (attributes.hasAttribute("RGB")) + { + std::string imgpath = attributes.value("RGB").toString().toStdString(); + printf("rgb filename: %s\n",(roomFolder+"/"+imgpath).c_str()); + rgb = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); + + //QString rgbpath = attributes.value("RGB").toString(); + //rgb = cv::imread(roomFolder+"/"+(rgbpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); + //rgb = cv::imread((rgbpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + if (attributes.hasAttribute("DEPTH")) + { + std::string imgpath = attributes.value("DEPTH").toString().toStdString(); + printf("depth filename: %s\n",(roomFolder+"/"+imgpath).c_str()); + depth = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); + //QString depthpath = attributes.value("DEPTH").toString(); + //printf("depth filename: %s\n",depthpath.toStdString().c_str()); + //depth = cv::imread((roomFolder+"/"+depthpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); + //depth = cv::imread(roomFolder+"/"+(depthpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + token = xmlReader->readNext();//Stamp + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//sec + elementName = xmlReader->name().toString(); + int sec = atoi(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//nsec + elementName = xmlReader->name().toString(); + int nsec = atoi(xmlReader->readElementText().toStdString().c_str()); + token = xmlReader->readNext();//end stamp + + token = xmlReader->readNext();//Camera + elementName = xmlReader->name().toString(); + + reglib::Camera * cam = new reglib::Camera(); + + token = xmlReader->readNext();//fx + cam->fx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//fy + cam->fy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cx + cam->cx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cy + cam->cy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Camera + elementName = xmlReader->name().toString(); + + double time = double(sec)+double(nsec)/double(1e9); + + token = xmlReader->readNext();//RegisteredPose + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d regpose = getPose(xmlReader); + + token = xmlReader->readNext();//RegisteredPose + elementName = xmlReader->name().toString(); + + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d pose = getPose(xmlReader); + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, pose); + frames.push_back(frame); + poses.push_back(pose); + } + } + } + delete xmlReader; +} + +std::vector readPoseXML(std::string xmlFile){ + std::vector poses; + QFile file(xmlFile.c_str()); + + if (!file.exists()){ + ROS_ERROR("Could not open file %s to load poses.",xmlFile.c_str()); + return poses; + } + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()){ + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()) + { + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + return poses; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "Pose"){ + Eigen::Matrix4d pose = getPose(xmlReader); + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + std::cout << pose << std::endl << std::endl; + poses.push_back(pose); + } + } + } + delete xmlReader; + printf("done readPoseXML\n"); + return poses; +} + +std::vector loadModels(std::string path){ + printf("loadModels: %s\n",path.c_str()); + std::vector models; + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + std::vector frames; + std::vector poses; + readViewXML(sweep_folder+"ViewGroup.xml",frames,poses); + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml")); + for (auto objectFile : objectFiles){ + std::string object = sweep_folder+objectFile.toStdString(); + printf("object: %s\n",object.c_str()); + + QFile file(object.c_str()); + + if (!file.exists()){ + ROS_ERROR("Could not open file %s to masks.",object.c_str()); + continue; + } + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()){ + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()){ + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + break; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "Mask"){ + int number = 0; + cv::Mat mask; + QXmlStreamAttributes attributes = xmlReader->attributes(); + if (attributes.hasAttribute("filename")){ + QString maskpath = attributes.value("filename").toString(); + //printf("mask filename: %s\n",(sweep_folder+maskpath.toStdString()).c_str()); + mask = cv::imread(sweep_folder+"/"+(maskpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); + //mask = cv::imread((maskpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + if (attributes.hasAttribute("image_number")){ + QString depthpath = attributes.value("image_number").toString(); + number = atoi(depthpath.toStdString().c_str()); + printf("number: %i\n",number); + }else{break;} + + mod->frames.push_back(frames[number]->clone()); + mod->relativeposes.push_back(poses[number]); + mod->modelmasks.push_back(new reglib::ModelMask(mask)); + } + } + } + + delete xmlReader; + models.push_back(mod); + } + + for(unsigned int i = 0; i < frames.size(); i++){delete frames[i];} + + return models; +} + +bool annotate(std::string path){ + printf("annotate: %s\n",path.c_str()); + std::vector models = loadModels(path); + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml")); + for (auto objectFile : objectFiles){ + std::string object = sweep_folder+objectFile.toStdString(); + printf("object: %s\n",object.c_str()); + } +} + +bool annotateFiles(std::string path){ + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(path); + for (auto sweep_xml : sweep_xmls) { + annotate(sweep_xml); + } + return false; +} + +int main(int argc, char** argv){ + visualization_lvl = 1; + + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + std::vector< std::string > folders; + int inputstate = 0; + for(int i = 1; i < argc;i++){ + printf("input: %s\n",argv[i]); + if(std::string(argv[i]).compare("-v") == 0){ inputstate = 1;} + else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 2;} + else if(inputstate == 1){visualization_lvl = atoi(argv[i]);} + else if(inputstate == 2){folders.push_back(std::string(argv[i]));} + } + + if(folders.size() == 0){folders.push_back(std::string(getenv ("HOME"))+"/.semanticMap/");} + + for(unsigned int i = 0; i < folders.size(); i++){annotateFiles(folders[i]);} + + + printf("done annotating\n"); + + return 0; +} diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index b50a28dd..ad67efc0 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -544,7 +544,7 @@ reglib::Model * processAV(std::string path){ std::vector both_unrefined; both_unrefined.push_back(Eigen::Matrix4d::Identity()); std::vector times; - for(unsigned int i = 0; i < 3 && i < viewrgbs.size(); i++){ + for(unsigned int i = 0; i < 3000 && i < viewrgbs.size(); i++){ printf("additional view: %i\n",i); geometry_msgs::TransformStamped msg; tf::transformStampedTFToMsg(viewtfs[i], msg); @@ -568,7 +568,7 @@ reglib::Model * processAV(std::string path){ reglib::Camera * cam = sweep->frames.front()->camera->clone(); reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m);//a.matrix()); frames.push_back(frame); - +/* //Visualize edges... viewer->removeAllPointClouds(); viewer->removeAllShapes(); @@ -605,66 +605,59 @@ reglib::Model * processAV(std::string path){ p.b = 0;//rgbdata[3*ind+0]; p.g = 0;//rgbdata[3*ind+1]; p.r = 0;//rgbdata[3*ind+2]; +// if(det_dilatedata[ind]){ +// p.r = 255; +// p.g = 255; +// p.b = 255; +// } cloud->points[ind] = p; } } } -viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + for(unsigned int w = 1; w < width; w++){ for(unsigned int h = 1; h < height;h++){ int ind = h*width+w; pcl::PointXYZRGB & p = cloud->points[ind]; - if(p.z > 1.5 || p.z == 0){ - p.r = 0; - p.g = 0; - p.b = 0; - }else{ - if(det_dilatedata[ind]){ - float px = 1.0-probs[0][ind]; - float py = 1.0-probs[1][ind]; - char buf [1024]; - { + float px = 1.0-probs[0][ind]; + float py = 1.0-probs[1][ind]; + + p.r = 255.0*(1-px); + p.g = 255.0*(1-py); + + if(det_dilatedata[ind]){ + + + char buf [1024]; - pcl::PointXYZRGB p1 = cloud->points[ind-1]; - double d = sqrt(pow(p.x-p1.x,2)+pow(p.y-p1.y,2)+pow(p.z-p1.z,2)); - if(d > 0.2 && px > 0.5){ - printf("%i , %i :: %i -> %i\n",w,h,ind,ind-1); + { + pcl::PointXYZRGB p1 = cloud->points[ind-1]; + double d = sqrt(pow(p.x-p1.x,2)+pow(p.y-p1.y,2)+pow(p.z-p1.z,2)); + if(((p1.z < 1.5 && p1.z > 0) && (p.z < 1.5 && p.z > 0)) && d > 0.05){// ){//){ + //printf("myset.insert(%i);\n",ind); sprintf(buf,"lineX_%i",ind); - viewer->addLine(p,p1,px,0,0,buf); - viewer->spin(); - } + viewer->addLine(p,p1,1-px,0,0,buf); + } + } + + { + pcl::PointXYZRGB p1 = cloud->points[ind-width]; + double d = sqrt(pow(p.x-p1.x,2)+pow(p.y-p1.y,2)+pow(p.z-p1.z,2)); + if(((p1.z < 1.5 && p1.z > 0) && (p.z < 1.5 && p.z > 0)) && d > 0.5){// ){//){ + //printf("myset.insert(%i);\n",ind); + sprintf(buf,"lineY_%i",ind); + viewer->addLine(p,p1,0,1-py,0,buf); } -// { -// pcl::PointXYZRGB p1 = cloud->points[ind-width]; -// sprintf(buf,"lineY_%i",ind); -// viewer->addLine(p,p1,0,1,0,buf); -// } - - // if(px > 0.1){ - // pcl::PointXYZRGB p1 = cloud->points[ind-1]; - // p1 = p; p1.x *= 1.1; p1.y *= 1.1; p1.z *= 1.1; - // //if(p1.z < 0.1){p1 = p; p1.x *= 1.1; p1.y *= 1.1; p1.z *= 1.1;} - - // sprintf(buf,"lineX_%i",ind); - // viewer->addLine(p,p1,1,0,0,buf); - // } - // if(py > 0.1){ - // pcl::PointXYZRGB p1 = cloud->points[ind-width]; - // p1 = p; p1.x *= 1.1; p1.y *= 1.1; p1.z *= 1.1; - // //if(p1.z < 0.1){p1 = p; p1.x *= 1.1; p1.y *= 1.1; p1.z *= 1.1;} - // sprintf(buf,"lineY_%i",ind); - // viewer->addLine(p,p1,0,1,0,buf); - // } } } } } - + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); viewer->spin(); - +*/ //both_unrefined.push_back(sweep->frames.front()->pose.inverse()*a.matrix()); both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); } diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index 4a4e16cc..a34733ce 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -62,6 +62,8 @@ using Components = boost::component_index; namespace reglib { +void pn(double p, unsigned int len = 3); + float graph_cut(std::vector& graphs_out,std::vector>& second_graphinds, Graph& graph_in, std::vector graph_inds); diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 4ca9166c..002cedde 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -105,9 +105,13 @@ class ModelUpdater{ virtual void getAreaWeights(Matrix4d p, RGBDFrame* frame1, double * weights1, double * overlaps1, double * total1, RGBDFrame* frame2, double * weights2, double * overlaps2, double * total2); + virtual void getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, double * notocclusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); + virtual void getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, std::vector & framesp1_test, std::vector & framesp1, double * overlaps, double * occlusions, double * notocclusions, RGBDFrame* frame2, std::vector & framesp2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); + + virtual void computeOcclusionAreas(vector cp, vector cf, vector cm); - virtual void getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); - virtual void getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, std::vector & framesp1_test, std::vector & framesp1, double * overlaps, double * occlusions, RGBDFrame* frame2, std::vector & framesp2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); +// virtual void getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); +// virtual void getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, std::vector & framesp1_test, std::vector & framesp1, double * overlaps, double * occlusions, RGBDFrame* frame2, std::vector & framesp2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg); virtual void getDynamicWeights(bool isbg, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * totalocclusions, RGBDFrame* frame2, cv::Mat mask, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength,double debugg = false); virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks); virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg = false); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 96597ce6..8752ec10 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -615,6 +615,10 @@ RGBDFrame * RGBDFrame::clone(){ //} RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals){ + + std::set myset; + + bool verbose = false; if(verbose) printf("------------------------------\n"); @@ -665,15 +669,78 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float * dedata = (float*)de.data; for(int i = 0; i < 3*nr_pixels; i++){dedata[i] = 0;} - std::vector Xvec; - int step = 1; for(unsigned int w = step; w < width-step;w++){ for(unsigned int h = step; h < height-step;h++){ int ind = h*width+w; float z = idepth*float(depthdata[ind]); + if(w > 2 && w < width-1){ + float z0 = idepth*float(depthdata[ind-2]); + float z1 = idepth*float(depthdata[ind-1]); + float z2 = z; + float z3 = idepth*float(depthdata[ind+1]); + + float pred1 = z1 + (z1-z0); + float pred2 = z2 - (z3-z2); + + if(pred1 > 0 || pred2 > 0){ + double n0 = z0*z0; + double n1 = z1*z1; + double n2 = z2*z2; + double n3 = z3*z3; + + double meanpred = 0.5*(fabs(z2-pred1) + fabs(z1-pred2)); + double noise3 = sqrt(n1*n1+n2*n2); + float pred3 = std::min(meanpred,fabs(z1-z2)); + dedata[3*ind+1] = pred3/noise3; + Xvec.push_back(meanpred/noise3); + +// if (myset.count(ind)!=0){ +// printf("%i :: ",ind); +// pn(z0); +// printf(" "); +// pn(z1); +// printf(" "); +// pn(z2); +// printf(" "); +// pn(z3); +// printf("-->"); +// pn(pred1); +// printf(" "); +// pn(pred2); +// printf("-->"); +// pn(Xvec.back()); +// printf("\n"); +// } + } + } + + if(h > 2 && h < height-1){ + float z0 = idepth*float(depthdata[ind-2*width]); + float z1 = idepth*float(depthdata[ind-1*width]); + float z2 = z; + float z3 = idepth*float(depthdata[ind+1*width]); + + float pred1 = z1 + (z1-z0); + float pred2 = z2 - (z3-z2); + + if(pred1 > 0 || pred2 > 0){ + double n0 = z0*z0; + double n1 = z1*z1; + double n2 = z2*z2; + double n3 = z3*z3; + + double meanpred = 0.5*(fabs(z2-pred1) + fabs(z1-pred2)); + double noise3 = sqrt(n1*n1+n2*n2); + float pred3 = std::min(meanpred,fabs(z1-z2)); + dedata[3*ind+2] = pred3/noise3; + Xvec.push_back(meanpred/noise3); + } + } + +/* if(w > 1){ float z2 = 0.5*idepth*float(depthdata[ind-step]+depthdata[ind+step]); double noise1 = z*z; @@ -699,6 +766,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt Xvec.push_back(dedata[3*ind+2]); } } +*/ + } } @@ -714,7 +783,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt DistanceWeightFunction2PPR3 * funcZ = new DistanceWeightFunction2PPR3(); funcZ->zeromean = true; - funcZ->startreg = 0.001; + //funcZ->startreg = 0.001; + funcZ->startreg = 0.000001; funcZ->debugg_print = false; funcZ->bidir = false; funcZ->maxp = 0.999999; @@ -739,71 +809,6 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt delete funcZ; - //////////////////////// - std::vector zsrc_dxdata; - zsrc_dxdata.resize(nr_pixels); - for(unsigned int i = 0; i < nr_pixels;i++){zsrc_dxdata[i] = 0;} - - std::vector zsrc_dydata; - zsrc_dydata.resize(nr_pixels); - for(unsigned int i = 0; i < nr_pixels;i++){zsrc_dydata[i] = 0;} - - std::vector zmaxima_dxdata; - zmaxima_dxdata.resize(nr_pixels); - for(unsigned int i = 0; i < nr_pixels;i++){zmaxima_dxdata[i] = 0;} - - std::vector zmaxima_dydata; - zmaxima_dydata.resize(nr_pixels); - for(unsigned int i = 0; i < nr_pixels;i++){zmaxima_dydata[i] = 0;} - - - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - - if(w > 1){ - float z2 = idepth*float(depthdata[ind-1]); - zsrc_dxdata[ind] += fabs(z-z2); - } - - if(h > 1){ - float z2 = idepth*float(depthdata[ind-width]); - zsrc_dydata[ind] += fabs(z-z2); - } - } - } - - - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - zmaxima_dxdata[ind] = (zsrc_dxdata[ind] > zsrc_dxdata[ind-1] && zsrc_dxdata[ind] >= zsrc_dxdata[ind+1]); - zmaxima_dydata[ind] = (zsrc_dydata[ind] > zsrc_dydata[ind-width] && zsrc_dydata[ind] >= zsrc_dydata[ind+width]); - } - } - -// cv::Mat zmax; -// zmax.create(height,width,CV_32FC3); -// float * zmaxdata = (float*)zmax.data; -// for(int i = 0; i < nr_pixels; i++){ -// zmaxdata[3*i+0] = 0; -// zmaxdata[3*i+1] = dedata[3*i+1]*zmaxima_dxdata[i]; -// zmaxdata[3*i+2] = dedata[3*i+2]*zmaxima_dydata[i]; -// } - -// // cv::namedWindow( "colour", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour", ce); -// // cv::namedWindow( "colour+nms", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour+nms", cenms); -// cv::namedWindow( "zmax", cv::WINDOW_AUTOSIZE ); cv::imshow( "zmax", zmax); -// cv::waitKey(0); - - for(int i = 0; i < nr_pixels; i++){ -// dedata[3*i+1] = std::max(dedata[3*i+1]*zmaxima_dxdata[i],0.0001f); -// dedata[3*i+2] = std::max(dedata[3*i+2]*zmaxima_dydata[i],0.0001f); - } - - //////////////////////// - if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); @@ -1016,7 +1021,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt ce = cenms; - int dilation_size = 4; + int dilation_size = 2; cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) ); unsigned char * det_dilatedata = (unsigned char*)det_dilate.data; @@ -1474,33 +1479,24 @@ std::vector< std::vector > RGBDFrame::getImageProbs(bool depthonly){ dyc.resize(nr_pixels); for(unsigned int i = 0; i < nr_pixels;i++){ -// dxc[i] = dedata[3*i+1]; -// dyc[i] = dedata[3*i+2]; - - if(!det_dilatedata[i]){ - dxc[i] = cedata[3*i+1]; - dyc[i] = cedata[3*i+2]; - }else{ - dxc[i] = std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); - dyc[i] = std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); - } + dxc[i] = dedata[3*i+1]; + dyc[i] = dedata[3*i+2]; - dxc[i] = std::min(dxc[i],dyc[i]); - dyc[i] = dxc[i];// std::min(probX,probY); + if(!depthonly){ + if(!det_dilatedata[i]){ + dxc[i] = std::max(dedata[3*i+1],cedata[3*i+1]); + dyc[i] = std::max(dedata[3*i+2],cedata[3*i+2]); + }else{ + dxc[i] = std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); + dyc[i] = std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); + } + } } -/* - - probY = 1-current; -} - -dxc[ind] = std::min(probX,probY); -dyc[ind] = std::min(probX,probY); -*/ // cv::namedWindow( "Colour edges" , cv::WINDOW_AUTOSIZE ); cv::imshow( "Colour edges", ce); // cv::namedWindow( "Depth edges" , cv::WINDOW_AUTOSIZE ); cv::imshow( "Depth edges", de); // cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); - +// cv::namedWindow( "det_dilate" , cv::WINDOW_AUTOSIZE ); cv::imshow( "det_dilate", det_dilate ); // cv::waitKey(0); std::vector< std::vector > probs2; diff --git a/quasimodo/quasimodo_models/src/core/Util.cpp b/quasimodo/quasimodo_models/src/core/Util.cpp index 2e217687..668699dc 100644 --- a/quasimodo/quasimodo_models/src/core/Util.cpp +++ b/quasimodo/quasimodo_models/src/core/Util.cpp @@ -53,6 +53,18 @@ namespace reglib{ return double(start1.tv_sec+(start1.tv_usec/1000000.0)); } + void pn(double p, unsigned int len){ + char buf1 [64]; + if(p >= 0){ + sprintf(buf1," %i.%if",len,len); + buf1[1] = '%'; + }else{ + sprintf(buf1," %i.%if",len,len); + buf1[0] = '%'; + } + printf(buf1,p); + } + float graph_cut(std::vector& graphs_out,std::vector>& second_graphinds, Graph& graph_in, std::vector graph_inds){ using adjacency_iterator = boost::graph_traits::adjacency_iterator; typename boost::property_map::type vertex_id = boost::get(boost::vertex_index, graph_in); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 8bd3bd7c..bd58c432 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -1906,7 +1906,7 @@ std::vector doInference(std::vector & prior, std::vector< std::vect return labels; } - +/* void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ std::vector framesp1_test = frame1->getSuperPoints(Eigen::Matrix4d::Identity(),10,false); std::vector rr_vec_test = frame2->getReprojections(framesp1_test,p,0,false); @@ -2033,6 +2033,163 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & viewer->spin(); } } +*/ + +void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, double * overlaps, double * occlusions, double * notocclusions, RGBDFrame* frame2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ + std::vector framesp1_test = frame1->getSuperPoints(Eigen::Matrix4d::Identity(),10,false); + std::vector rr_vec_test = frame2->getReprojections(framesp1_test,p,0,false); + + double inlierratio = double(rr_vec_test.size())/double(framesp1_test.size()); + + if( inlierratio < 0.01 ){return ;} + std::vector framesp1 = frame1->getSuperPoints(); + std::vector framesp2 = frame2->getSuperPoints();//p); + + getDynamicWeights(store_distance,dvec,nvec,dfunc,nfunc,p,frame1,framesp1_test,framesp1,overlaps,occlusions,notocclusions,frame2,framesp2,offset1,offset2,interframe_connectionId, interframe_connectionStrength, debugg); +} + +void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & dvec, std::vector & nvec, DistanceWeightFunction2 * dfunc, DistanceWeightFunction2 * nfunc, Matrix4d p, RGBDFrame* frame1, std::vector & tframesp1_test, std::vector & tframesp1, double * overlaps, double * occlusions, double * notocclusions, RGBDFrame* frame2, std::vector & tframesp2, int offset1, int offset2, std::vector< std::vector > & interframe_connectionId, std::vector< std::vector > & interframe_connectionStrength, double debugg){ + double totalstartTime = getTime(); + + std::string currentString; + double startTime; + startTime = getTime(); + unsigned char * src_detdata = (unsigned char*)(frame1->det_dilate.data); + unsigned char * dst_detdata = (unsigned char*)(frame2->det_dilate.data); + + std::vector & framesp1_test = tframesp1_test;//frame1->getSuperPoints(Eigen::Matrix4d::Identity(),10,false); + std::vector rr_vec_test = frame2->getReprojections(framesp1_test,p,0,false); + double inlierratio = double(rr_vec_test.size())/double(framesp1_test.size()); + + currentString = "part1"; + if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;} + benchtime[currentString] += getTime() - startTime; + startTime = getTime(); + + if( inlierratio < 0.01 ){return ;} + + std::vector & framesp1 = tframesp1;//frame1->getSuperPoints(); + std::vector & framesp2 = tframesp2;//frame2->getSuperPoints(); + + currentString = "part2"; + if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;} + benchtime[currentString] += getTime() - startTime; + startTime = getTime(); + + std::vector rr_vec = frame2->getReprojections(framesp1,p,0,false); + inlierratio = double(rr_vec.size())/double(framesp1.size()); + unsigned long nr_rr = rr_vec.size(); + + currentString = "part3"; + if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;} + benchtime[currentString] += getTime() - startTime; + startTime = getTime(); + + pcl::PointCloud::Ptr src_cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dst_cloud (new pcl::PointCloud); + if(debugg){ + src_cloud = getPointCloudFromVector(framesp1,3,255,0,255); + dst_cloud = getPointCloudFromVector(framesp2,3,0,0,255); + } + + double threshold = 0; + if(!store_distance){ + threshold = pow(20.0*dfunc->getNoise(),2); + } + + int totsum = 0; + for(unsigned long ind = 0; ind < nr_rr;ind++){ + ReprojectionResult & rr = rr_vec[ind]; + unsigned int src_ind = rr.src_ind; + superpoint & src_p = framesp1[src_ind]; + if(src_p.point(2) < 0.0){continue;} + + totsum++; + unsigned int dst_ind = rr.dst_ind; + superpoint & dst_p = framesp2[dst_ind]; + double src_variance = 1.0/src_p.point_information; + double dst_variance = 1.0/dst_p.point_information; + double total_variance = src_variance+dst_variance; + double total_stdiv = sqrt(total_variance); + double d = rr.residualZ/total_stdiv; + double surface_angle = rr.angle; + if(store_distance){ + dvec.push_back(d); + nvec.push_back(1-surface_angle); + }else{ + double dE2 = rr.residualE2/total_stdiv; + double p_overlap_angle = nfunc->getProb(1-surface_angle); + double p_overlap = dfunc->getProb(d); + double p_occlusion = dfunc->getProbInfront(d);//std::max(1 - 1e-6,std::max(1e-6,dfunc->getProbInfront(d))); + + p_overlap *= p_overlap_angle; + + if(p_overlap > 0.001 && offset1 >= 0 && offset2 >= 0 && dE2 < threshold){ + interframe_connectionId[offset1+src_ind].push_back(offset2+dst_ind); + interframe_connectionStrength[offset1+src_ind].push_back(-log(1-p_overlap)); + } + + if(debugg){ + src_cloud->points[src_ind].r = 255.0 * p_overlap; + src_cloud->points[src_ind].g = 255.0 * p_occlusion; + src_cloud->points[src_ind].b = 0; + } + + if(dst_detdata[dst_ind] != 0){continue;} + if(src_detdata[src_ind] != 0){continue;} + + overlaps[src_ind] = std::min(0.9,std::max(overlaps[src_ind],p_overlap)); + //double prev = 1.0-occlusions[src_ind]; + occlusions[src_ind] += p_occlusion;//std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); + notocclusions[src_ind]++; + //notocclusions[src_ind] *= 1.0-p_occlusion;//std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); + + + //occlusions[src_ind] *= p_occlusion;//std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); + //notocclusions[src_ind] *= 1.0-p_occlusion;//std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); + } + } + currentString = "part4"; + if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;} + benchtime[currentString] += getTime() - startTime; + startTime = getTime(); + + currentString = "total_getDyn"; + if(benchtime.count(currentString) == 0){benchtime[currentString] = 0;} + benchtime[currentString] += getTime() - totalstartTime; + startTime = getTime(); + + if(false && !store_distance){ + cv::Mat overlapsimg; + overlapsimg.create(frame1->camera->height,frame1->camera->width,CV_32FC3); + float * overlapsimgdata = (float*)overlapsimg.data; + cv::Mat occlusionsimg; + occlusionsimg.create(frame1->camera->height,frame1->camera->width,CV_32FC3); + float * occlusionsimgdata = (float*)occlusionsimg.data; + for(unsigned long ind = 0; ind < frame1->camera->height*frame1->camera->width;ind++){ + overlapsimgdata[3*ind+0] = overlaps[ind]; + overlapsimgdata[3*ind+1] = overlaps[ind]; + overlapsimgdata[3*ind+2] = overlaps[ind]; + + occlusionsimgdata[3*ind+0] = occlusions[ind]/std::max(1.0,notocclusions[ind]); + occlusionsimgdata[3*ind+1] = occlusionsimgdata[3*ind+0]; + occlusionsimgdata[3*ind+2] = occlusionsimgdata[3*ind+0]; + } + cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame1->rgb ); + cv::namedWindow( "occlusionsimg", cv::WINDOW_AUTOSIZE ); cv::imshow( "occlusionsimg",occlusionsimg ); + cv::namedWindow( "overlapsimg", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlapsimg", overlapsimg ); + cv::waitKey(0); + } + + if(debugg && totsum > 0){ + pcl::PointCloud::Ptr src_cloud = getPointCloudFromVector(framesp1,3,255,0,255); + pcl::PointCloud::Ptr dst_cloud = getPointCloudFromVector(framesp2,3,0,0,255); + viewer->removeAllPointClouds(); + viewer->addPointCloud (src_cloud, pcl::visualization::PointCloudColorHandlerRGBField(src_cloud), "scloud"); + viewer->addPointCloud (dst_cloud, pcl::visualization::PointCloudColorHandlerRGBField(dst_cloud), "dcloud"); + viewer->spin(); + } +} void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ double computeMovingDynamicStatic_startTime = getTime(); @@ -2048,6 +2205,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s tot_nr_pixels += nr_pixels; } + std::vector labels; + labels.resize(tot_nr_pixels); + //Graph... std::vector< std::vector > interframe_connectionId; std::vector< std::vector > interframe_connectionStrength; @@ -2089,7 +2249,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s if(i == j){continue;} Eigen::Matrix4d p = poses[i].inverse() * poses[j]; std::vector & framesp2 = framesp[j]; - getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp1_test,framesp1, 0, 0, frames[j],framesp2,offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); + getDynamicWeights(true,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp1_test,framesp1, 0, 0, 0, frames[j],framesp2,offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); } } @@ -2181,57 +2341,101 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s unsigned int nr_pixels = width * height; double * current_overlaps = new double[nr_pixels]; double * current_occlusions = new double[nr_pixels]; + double * current_notocclusions = new double[nr_pixels]; for(unsigned int j = 0; j < nr_pixels; j++){ current_overlaps[j] = 0; - current_occlusions[j] = 0; + current_occlusions[j] = 0.0; + current_notocclusions[j] = 0.0; } double * bg_overlaps = new double[nr_pixels]; double * bg_occlusions = new double[nr_pixels]; + double * bg_notocclusions = new double[nr_pixels]; for(unsigned int j = 0; j < nr_pixels; j++){ - bg_overlaps[j] = 0; - bg_occlusions[j] = 0; + bg_overlaps[j] = 0; + bg_occlusions[j] = 0.0; + bg_notocclusions[j] = 0.0; } total_alloctime += getTime()-startTime; startTime = getTime(); + printf("current\n"); for(unsigned int j = 0; j < frames.size(); j++){ if(i == j){continue;} Eigen::Matrix4d p = poses[i].inverse() * poses[j]; - getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], current_overlaps, current_occlusions, frames[j],framesp[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); + getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], current_overlaps, current_occlusions, current_notocclusions, frames[j],framesp[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); } + printf("background\n"); for(unsigned int j = 0; j < bgcf.size(); j++){ Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; - getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], bg_overlaps, bg_occlusions, bgcf[j],bgsp[j],-1,-1,interframe_connectionId,interframe_connectionStrength,false); + getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], bg_overlaps, bg_occlusions, bg_notocclusions, bgcf[j],bgsp[j],-1,-1,interframe_connectionId,interframe_connectionStrength,false); + } + + for(unsigned int j = 0; j < nr_pixels; j++){ + bg_occlusions[j] = std::min(0.99,bg_occlusions[j]/std::max(1.0,bg_notocclusions[j])); + current_occlusions[j] = std::min(0.99,current_occlusions[j]/std::max(1.0,current_notocclusions[j])); + //bg_occlusions[j] = bg_occlusions[j] /bg_notocclusions[j]; + //current_occlusions[j] = current_occlusions[j] /current_notocclusions[j]; } + +// for(unsigned int j = 0; j < nr_pixels; j++){ +// bg_occlusions[j] = bg_occlusions[j] /(bg_occlusions[j] +bg_notocclusions[j]); +// current_occlusions[j] = current_occlusions[j] /(current_occlusions[j] +current_notocclusions[j]); +// } total_Dynw += getTime()-startTime; + cv::Mat priorsimg; + priorsimg.create(height,width,CV_32FC3); + float * priorsdata = (float*)priorsimg.data; + //for(int i = 0; i < 3*nr_pixels; i++){priorsdata[i] = priors[3*offset+i];} startTime = getTime(); unsigned char * detdata = (unsigned char*)(frame->det_dilate.data); - float minprob = 0.01; + float bias = 0.01; for(unsigned int h = 0; h < height;h++){ for(unsigned int w = 0; w < width;w++){ int ind = h*width+w; valids[offset+ind] = detdata[ind] == 0; + if(valids[offset+ind]){ + float p_moving = current_occlusions[ind]; + float p_dynamic = std::max((bg_occlusions[ind]-p_moving)*current_overlaps[ind],0.0); + p_dynamic += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); + p_moving += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); - float p_moving = current_occlusions[ind]; - float p_dynamic = std::max((bg_occlusions[ind]-p_moving)*current_overlaps[ind],0.0); - p_dynamic += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); - p_moving += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); + float p_static = std::max(bg_overlaps[ind]-p_moving-p_dynamic,0.0); + float leftover = 1-p_moving-p_dynamic-p_static; - float p_static = std::max(bg_overlaps[ind]-p_moving-p_dynamic,0.0); + float p_dyn = p_dynamic + (0.5-bias)*leftover; + float p_other = p_static+p_moving + (0.5+bias)*leftover; - float leftover = 1-p_moving-p_dynamic-p_static; + priorsdata[3*ind+2] = p_dyn;//p_moving; + priorsdata[3*ind+1] = p_dyn; + priorsdata[3*ind+0] = p_dyn; + + priors[3*(offset+ind)+0] = 0; + priors[3*(offset+ind)+1] = p_dyn; + priors[3*(offset+ind)+2] = p_other; + }else{ + + priorsdata[3*ind+2] = 0;//p_moving; + priorsdata[3*ind+1] = 0.5; + priorsdata[3*ind+0] = 0.5; + + priors[3*(offset+ind)+0] = 0; + priors[3*(offset+ind)+1] = 0.5; + priors[3*(offset+ind)+2] = 0.5; + } + + /* float notMoving = current_overlaps[ind]; float bias = 0.0001; - float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; - float p_dynamic_leftover = -bias+0.5*leftover*notMoving + (1-notMoving)*leftover/4.0; - float p_static_leftover = 2.0*bias+0.5*leftover*notMoving + (1-notMoving)*leftover/2.0; + float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; + float p_dynamic_leftover = -bias+0.5*notMoving*leftover + (1-notMoving)*leftover/4.0; + float p_static_leftover = 2.0*bias+0.5*notMoving*leftover + (1-notMoving)*leftover/2.0; float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover) + minprob; float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover) + minprob; @@ -2240,6 +2444,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s priors[3*(offset+ind)+0] = p_moving_tot; priors[3*(offset+ind)+1] = p_dynamic_tot; priors[3*(offset+ind)+2] = p_static_tot; +*/ + if(probs[0][ind] > 0.00000001){frameConnections++;} if(probs[1][ind] > 0.00000001){frameConnections++;} @@ -2248,6 +2454,72 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } + printf("run inference\n"); + double start_inf = getTime(); + gc::Graph * g = new gc::Graph(nr_pixels,2*nr_pixels); + for(unsigned long ind = 0; ind < nr_pixels;ind++){ + g -> add_node(); + double p_fg = priors[3*(offset+ind)+1]; + double p_bg = priors[3*(offset+ind)+2]; + double norm = p_fg + p_bg; + if(norm <= 0){ + g -> add_tweights( ind, 0, 0 ); + continue; + } + p_fg /= norm; + p_bg /= norm; + + double weightFG = -log(p_fg); + double weightBG = -log(p_bg); + g -> add_tweights( ind, weightFG, weightBG ); + } + + + double maxprob_same = 0.99999999999; + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + if(w > 0 && probs[0][ind] > 0.00000001){ + int other = ind-1; + double p_same = std::min(double(probs[0][ind]),maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + g -> add_edge( ind, other, weight, weight ); + } + + if(h > 0 && probs[1][ind] > 0.00000001){ + int other = ind-width; + double p_same = std::min(double(probs[1][ind]),maxprob_same); + double not_p_same = 1-p_same; + double weight = -log(not_p_same); + g -> add_edge( ind, other, weight, weight ); + } + } + } + + g -> maxflow(); + for(unsigned long ind = 0; ind < nr_pixels;ind++){labels[offset+ind] = g->what_segment(ind);} + + printf("local inference time: %10.10fs\n\n",getTime()-start_inf); + +// cv::Mat labelimg; +// labelimg.create(height,width,CV_32FC3); +// float * labelimgdata = (float*)labelimg.data; +// for(unsigned long ind = 0; ind < nr_pixels;ind++){ +// double label = g->what_segment(ind); +// labelimgdata[3*ind+0] = 0; +// labelimgdata[3*ind+1] = label; +// labelimgdata[3*ind+2] = 1-label; +// } +// cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb ); +// cv::namedWindow( "depth" , cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", frame->depth ); +// cv::namedWindow( "priors" , cv::WINDOW_AUTOSIZE ); cv::imshow( "priors", priorsimg ); +// cv::namedWindow( "labelimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "labelimg", labelimg ); +// cv::waitKey(0); + + delete g; + Eigen::Matrix4d p = poses[i]; float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); @@ -2272,8 +2544,10 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s startTime = getTime(); delete[] current_occlusions; + delete[] current_notocclusions; delete[] current_overlaps; delete[] bg_occlusions; + delete[] bg_notocclusions; delete[] bg_overlaps; total_dealloctime += getTime()-startTime; } @@ -2288,26 +2562,23 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s printf("total_Dynw = %5.5fs\n", total_Dynw); long interframeConnections = 0; - for(unsigned int i = 0; i < interframe_connectionId.size();i++){ - for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ - interframeConnections++; - } - } + for(unsigned int i = 0; i < interframe_connectionId.size();i++){interframeConnections += interframe_connectionId[i].size();} double start_inf = getTime(); gc::Graph * g = new gc::Graph(current_point,frameConnections+interframeConnections); for(unsigned long i = 0; i < current_point;i++){ g -> add_node(); - double p_fg = priors[3*i+0]+priors[3*i+1]; + double p_fg = priors[3*i+1]; double p_bg = priors[3*i+2]; double norm = p_fg + p_bg; - p_fg /= norm; - p_bg /= norm; - if(priors[3*i+0]+priors[3*i+1]+priors[3*i+2] <= 0){ + if(norm <= 0){ g -> add_tweights( i, 0, 0 ); continue; } + p_fg /= norm; + p_bg /= norm; + double weightFG = -log(p_fg); double weightBG = -log(p_bg); g -> add_tweights( i, weightFG, weightBG ); @@ -2341,6 +2612,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } } + printf("run inference\n"); std::vector > interframe_connectionId_added; @@ -2349,6 +2621,279 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s std::vector > interframe_connectionStrength_added; interframe_connectionStrength_added.resize(interframe_connectionStrength.size()); + double initAdded = 0; + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + double weight = interframe_connectionStrength[i][j]; + unsigned long other = interframe_connectionId[i][j]; + //if(weight > 0.01 && fabs(priors[3*i+1] - priors[3*i+2]) > 0.5){//labels[i] != labels[other]){//g->what_segment(i) != g->what_segment(other)){ + if(weight > 0.01 && labels[i] != labels[other]){//g->what_segment(i) != g->what_segment(other)){ + g -> add_edge( i, other, weight, weight ); + + interframe_connectionId_added[i].push_back(other); + interframe_connectionStrength_added[i].push_back(weight); + + interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back(); + interframe_connectionStrength[i].pop_back(); + + interframe_connectionId[i][j] = interframe_connectionId[i].back(); + interframe_connectionId[i].pop_back(); + j--; + + initAdded++; + } + } + } + + printf("initAdded %f of %i\n",initAdded,interframeConnections); + + g -> maxflow(); + for(unsigned long ind = 0; ind < current_point;ind++){labels[ind] = g->what_segment(ind);} + + double tot_inf = 0; + for(unsigned int it = 0; it < 40; it++){ + double start_inf1 = getTime(); + + double diffs = 0; + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + double weight = interframe_connectionStrength[i][j]; + unsigned long other = interframe_connectionId[i][j]; + if(weight > 0.01 && labels[i] != labels[other]){diffs++;} + } + } + + + double adds = 100000; + double prob = std::min(adds / diffs,1.0); + printf("diffs: %f adds: %f prob: %f\n",diffs,adds,prob); + printf("ratio of total diffs: %f\n",diffs/double(frameConnections+interframeConnections)); + + if(diffs == 0){break;} + + double trueadds = 0; + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ + double weight = interframe_connectionStrength[i][j]; + unsigned long other = interframe_connectionId[i][j]; + if(weight > 0.01 && labels[i] != labels[other]){//g->what_segment(i) != g->what_segment(other)){ + if(rand() <= prob*RAND_MAX){ + trueadds++; + g -> add_edge( i, other, weight, weight ); + + interframe_connectionId_added[i].push_back(other); + interframe_connectionStrength_added[i].push_back(weight); + + interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back(); + interframe_connectionStrength[i].pop_back(); + + interframe_connectionId[i][j] = interframe_connectionId[i].back(); + interframe_connectionId[i].pop_back(); + j--; + } + } + } + } + printf("trueadds: %f\n",trueadds); + + g -> maxflow(); + for(unsigned long ind = 0; ind < current_point;ind++){labels[ind] = g->what_segment(ind);} + + tot_inf += getTime()-start_inf1; + printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf); + + if(tot_inf > 90){break;} + } + + double interfrace_constraints_added = 0; + for(unsigned int i = 0; i < interframe_connectionId.size();i++){ + interfrace_constraints_added += interframe_connectionId_added[i].size(); + for(unsigned int j = 0; j < interframe_connectionId_added[i].size();j++){ + interframe_connectionStrength[i].push_back(interframe_connectionStrength_added[i][j]); + interframe_connectionId[i].push_back(interframe_connectionId_added[i][j]); + } + } + + delete g; + double end_inf = getTime(); + printf("static inference time: %10.10fs interfrace_constraints added ratio: %f\n",end_inf-start_inf,interfrace_constraints_added/double(interframeConnections)); + + if(false){ + printf("3-class\n"); + for(unsigned int i = 0; i < current_point; i++){ + cloud->points[i].r = priors[3*i+0]*255.0; + cloud->points[i].g = priors[3*i+1]*255.0; + cloud->points[i].b = priors[3*i+2]*255.0; + } + + pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); + cloud_sample->points.clear(); + for(unsigned int i = 0; i < current_point; i++){ + cloud_sample->points.push_back(cloud->points[i]); + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + + for(unsigned int i = 0; i < current_point; i++){ + cloud_sample->points[i].r = 255*labels[i]; + cloud_sample->points[i].g = 255*(1-labels[i]); + cloud_sample->points[i].b = 0; + } + + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + } + + double start_inf1 = getTime(); + + const unsigned int nr_frames = frames.size(); + const unsigned int width = frames[0]->camera->width; + const unsigned int height = frames[0]->camera->height; + const unsigned int pixels_per_image = width*height; + const unsigned int nr_pixels = nr_frames*pixels_per_image; + + double probthresh = 0.5; + double str_probthresh = -log(probthresh); + unsigned int number_of_dynamics = 0; + unsigned int nr_obj_dyn = 0; + unsigned int nr_obj_mov = 0; + std::vector objectlabel; + std::vector labelID; + labelID.push_back(0); + objectlabel.resize(nr_pixels); + + for(unsigned long i = 0; i < nr_pixels; i++){objectlabel[i] = 0;} + for(unsigned long ind = 0; ind < nr_pixels; ind++){ + if(valids[ind] && objectlabel[ind] == 0 && labels[ind] != 0){ + unsigned int current_label = labels[ind]; + number_of_dynamics++; + objectlabel[ind] = number_of_dynamics; + unsigned long todocounter = 0; + std::vector< unsigned long > todo; + todo.push_back(ind); + double score = 0; + double totsum = 0; + while(todocounter < todo.size()){ + unsigned long cind = todo[todocounter++]; + unsigned long frameind = cind / pixels_per_image; + + + unsigned long iind = cind % pixels_per_image; + unsigned long w = iind % width; + unsigned long h = iind / width; + + double p0 = priors[3*cind+0]; + double p1 = priors[3*cind+1]; + double p2 = priors[3*cind+2]; + + if(valids[cind]){ + double s = 0; + if(p0 > p2){s += p1 - p0;} + else{ s += p1 - p2;} + score += s; + totsum++; + } + + float * dedata = (float*)(frames[frameind]->de.data); + + int dir; + dir = -1; + if( w > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + dir = 1; + if( w < (width-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + dir = -width; + if( h > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + dir = width; + if( h < (height-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + objectlabel[cind+dir] = number_of_dynamics; + todo.push_back(cind+dir); + } + + for(unsigned long j = 0; j < interframe_connectionId[cind].size();j++){ + unsigned long other = interframe_connectionId[cind][j]; + if(interframe_connectionStrength[cind][j] > str_probthresh && objectlabel[other] == 0 && labels[other] == current_label){ + objectlabel[other] = number_of_dynamics; + todo.push_back(other); + } + } + } + + printf("score: %f\n",score); + + + labelID.push_back(0); + if(score < 200){continue;} + + if(debugg && todo.size() > 10000){ + for(unsigned int i = 0; i < current_point; i++){ + cloud->points[i].r = priors[3*i+0]*255.0; + cloud->points[i].g = priors[3*i+1]*255.0; + cloud->points[i].b = priors[3*i+2]*255.0; + } + + pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); + cloud_sample->points.clear(); + for(unsigned int i = 0; i < current_point; i++){ + cloud_sample->points.push_back(cloud->points[i]); + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + + for(unsigned int i = 0; i < current_point; i++){ + cloud_sample->points[i].r = 255; + cloud_sample->points[i].g = 0; + cloud_sample->points[i].b = 0; + } + + for(unsigned int j = 0; j < todo.size(); j++){ + unsigned int i = todo[j]; + cloud_sample->points[i].r = 0; + cloud_sample->points[i].g = 255; + cloud_sample->points[i].b = 0; + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + } + + if(score < 0){continue;} + if(current_label == 1){ + labelID.back() = ++nr_obj_dyn; + printf("Dynamic: %f -> %f\n",score,totsum); + sr.component_dynamic.push_back(todo); + sr.scores_dynamic.push_back(score); + sr.total_dynamic.push_back(totsum); + } + if(current_label == 2){ + labelID.back() = --nr_obj_mov; + printf("Moving: %f -> %f\n",score,totsum); + sr.component_moving.push_back(todo); + sr.scores_moving.push_back(score); + sr.total_moving.push_back(totsum); + } + } + } + interframe_connectionId.clear(); + interframe_connectionStrength.clear(); + + printf("connectedComponent: %5.5fs\n",getTime()-start_inf); + +/* double tot_inf = 0; for(unsigned int it = 0; it < 10; it++){ double start_inf1 = getTime(); @@ -2412,10 +2957,10 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } int dynamic_label = 1; - std::vector labels; + //std::vector labels; std::vector dyn_ind; std::vector stat_ind; - labels.resize(current_point); + //labels.resize(current_point); stat_ind.resize(current_point); long nr_dynamic = 0; for(unsigned long i = 0; i < current_point;i++){ @@ -2767,7 +3312,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s interframe_connectionStrength.clear(); printf("connectedComponent: %5.5fs\n",getTime()-start_inf); - +*/ int current = 0; for(unsigned long i = 0; i < frames.size(); i++){ Camera * camera = frames[i]->camera; @@ -2806,7 +3351,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s if(debugg){ pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); - printf("3-class\n"); for(unsigned int i = 0; i < current_point; i++){ cloud->points[i].r = priors[3*i+0]*255.0; cloud->points[i].g = priors[3*i+1]*255.0; @@ -2862,6 +3406,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s viewer->spin(); } + delete[] valids; delete[] priors; printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime); } From f281d194a2434e076daa5c169e3eb3887ecc7c62 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 19 Oct 2016 23:10:40 +0200 Subject: [PATCH 168/255] .... --- .../quasimodo_brain/src/AnnotationTool.cpp | 250 +++++++++++++++++- quasimodo/quasimodo_brain/src/Util/Util.cpp | 2 +- .../metaroom_additional_view_processing.cpp | 181 ++++++------- 3 files changed, 323 insertions(+), 110 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp index 748e512a..74120b23 100644 --- a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp +++ b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp @@ -338,11 +338,14 @@ std::vector loadModels(std::string path){ int slash_pos = path.find_last_of("/"); std::string sweep_folder = path.substr(0, slash_pos) + "/"; + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml")); + if(objectFiles.size() == 0){return models;} + std::vector frames; std::vector poses; readViewXML(sweep_folder+"ViewGroup.xml",frames,poses); - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml")); + for (auto objectFile : objectFiles){ std::string object = sweep_folder+objectFile.toStdString(); printf("object: %s\n",object.c_str()); @@ -409,14 +412,253 @@ std::vector loadModels(std::string path){ bool annotate(std::string path){ printf("annotate: %s\n",path.c_str()); - std::vector models = loadModels(path); + + std::string xmlFile = path; + std::vector< std::string > rgbpaths; + + + QString xmlFileQS(xmlFile.c_str()); + int index = xmlFileQS.lastIndexOf('/'); + std::string roomFolder = xmlFileQS.left(index).toStdString(); + + + + QFile file((roomFolder+"/ViewGroup.xml").c_str()); + if (!file.exists()){ + ROS_ERROR("Could not open file %s to load room.",(roomFolder+"/ViewGroup.xml").c_str()); + return false; + } + + file.open(QIODevice::ReadOnly); + + ROS_INFO_STREAM("Parsing xml file: "<<(roomFolder+"/ViewGroup.xml").c_str()); + + QXmlStreamReader* xmlReader = new QXmlStreamReader(&file); + + while (!xmlReader->atEnd() && !xmlReader->hasError()){ + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()){ + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + return false; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "View"){ + QXmlStreamAttributes attributes = xmlReader->attributes(); + if (attributes.hasAttribute("RGB")){ + rgbpaths.push_back(roomFolder+"/"+attributes.value("RGB").toString().toStdString()); + printf("%s\n",rgbpaths.back().c_str()); + }else{break;} + } + } + } + + delete xmlReader; + + printf("rgbpaths: %i\n",rgbpaths.size()); + + QStringList objectFiles = QDir(roomFolder.c_str()).entryList(QStringList("dynamic_obj*.xml")); + if(objectFiles.size() == 0){return true;} + + + + for (auto objectFile : objectFiles){ + std::string object = roomFolder+"/"+objectFile.toStdString(); + printf("object: %s\n",object.c_str()); + + std::vector objectMasks; + std::vector imgNumber; + unsigned int current_displayInd = 0; + unsigned int maxNrPixels = 0; + + QFile objfile(object.c_str()); + + if (!objfile.exists()){ + ROS_ERROR("Could not open file %s to masks.",object.c_str()); + continue; + } + + objfile.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !objxmlReader->hasError()){ + QXmlStreamReader::TokenType token = objxmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (objxmlReader->hasError()){ + ROS_ERROR("XML error: %s",objxmlReader->errorString().toStdString().c_str()); + break; + } + + QString elementName = objxmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement){ + if (objxmlReader->name() == "Mask"){ + int number = 0; +// cv::Mat mask; + QXmlStreamAttributes attributes = objxmlReader->attributes(); + if (attributes.hasAttribute("filename")){ + QString maskpath = attributes.value("filename").toString(); + objectMasks.push_back(cv::imread(roomFolder+"/"+(maskpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED)); + unsigned char * data = objectMasks.back().data; + unsigned int nr_points = objectMasks.back().rows * objectMasks.back().cols; + unsigned int nrPixels = 0; + for(unsigned int ind = 0; ind < nr_points; ind++ ){nrPixels += data[ind] != 0;} + + if( maxNrPixels < nrPixels ){ + maxNrPixels = nrPixels; + current_displayInd = objectMasks.size()-1; + } + }else{break;} + + + if (attributes.hasAttribute("image_number")){ + QString depthpath = attributes.value("image_number").toString(); + number = atoi(depthpath.toStdString().c_str()); + printf("number: %i\n",number); + imgNumber.push_back(number); + if(imgNumber.back() >= rgbpaths.size()){return false;} + }else{break;} + } + } + } + + if(objectMasks.size() != imgNumber.size()){return false;} + + std::string classname = ""; + std::string instancename = ""; + int fontFace = cv::FONT_HERSHEY_COMPLEX_SMALL; + double fontScale = 1; + int thickness = 1; + int state = 0; + + cv::Mat rgb = cv::imread(rgbpaths[imgNumber[current_displayInd]], CV_LOAD_IMAGE_UNCHANGED); + cv::Mat mask = objectMasks[current_displayInd]; + while(true){ + + + std::vector > contours; + std::vector hierarchy; + + cv::findContours( mask, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) ); + for( unsigned int i = 0; i < contours.size(); i++ ){ + cv::drawContours( rgb, contours, i, cv::Scalar( 0, 0, 255 ), 3, 8, hierarchy, 0, cv::Point() ); + cv::drawContours( rgb, contours, i, cv::Scalar( 0, 255, 0 ), 1, 8, hierarchy, 0, cv::Point() ); + } + + + unsigned int height = rgb.rows; + unsigned int width = rgb.cols; + unsigned int newwidth = width+400; + + + unsigned char * rgbdata = rgb.data; + + cv::Mat img; + img.create(height,newwidth,CV_8UC3); + unsigned char * imgdata = img.data; + + for(unsigned int i = 0; i < 3*height*newwidth; i++){imgdata[i] = 0;} + + + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + int oind = h*width+w; + int nind = h*newwidth+w; + imgdata[3*nind + 0] = rgbdata[3*oind + 0]; + imgdata[3*nind + 1] = rgbdata[3*oind + 1]; + imgdata[3*nind + 2] = rgbdata[3*oind + 2]; + } + } + + + int textnr = 0; + putText(img, " Class: "+classname, cv::Point(width+10, 30+(textnr++)*25 ), fontFace, fontScale, cv::Scalar::all(255), thickness, 8); + putText(img, " Instance: "+instancename, cv::Point(width+10, 30+(textnr++)*25 ), fontFace, fontScale, cv::Scalar::all(255), thickness, 8); + + + char buf [1024]; + sprintf(buf," Image: %i / %i",current_displayInd+1,objectMasks.size()); + putText(img, std::string(buf), cv::Point(width+10, 30+(textnr++)*25 ), fontFace, fontScale, cv::Scalar::all(255), thickness, 8); + + putText(img, "--", cv::Point(width+5, 30+state*25 ), fontFace, fontScale, cv::Scalar(0,255,0), thickness, 8); + + cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb",img ); + char c = cv::waitKey(0); + printf("c: %c -> %i\n",c,int(c)); + + if(c == -29){ + state = 3; + }else if(state == 0){ + if((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '1')){classname += c;} + if(c == 8 && classname.size() > 0){classname.pop_back();} + if(c == 10){state = 1;} + }else if(state == 1){ + if((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '1')){instancename += c;} + if(c == 8 && instancename.size() > 0){instancename.pop_back();} + if(c == 10){break;} + }else if(state == 2){ + if(c == 'q' || c == 'Q'){ + current_displayInd = std::max(int(current_displayInd-1),0); + rgb = cv::imread(rgbpaths[imgNumber[current_displayInd]], CV_LOAD_IMAGE_UNCHANGED); + mask = objectMasks[current_displayInd]; + } + if(c == 'w' || c == 'W'){ + current_displayInd = std::min(int(current_displayInd+1),int(imgNumber.size()-1)); + rgb = cv::imread(rgbpaths[imgNumber[current_displayInd]], CV_LOAD_IMAGE_UNCHANGED); + mask = objectMasks[current_displayInd]; + } + if(c == 10){state = 0;} + }else if(state == 3){ + if(c == '1'){state = 0;} + if(c == '2'){state = 1;} + if(c == '3'){state = 2;} + } + + } + + + + delete objxmlReader; + } + +/* + int slash_pos = path.find_last_of("/"); std::string sweep_folder = path.substr(0, slash_pos) + "/"; QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml")); - for (auto objectFile : objectFiles){ - std::string object = sweep_folder+objectFile.toStdString(); + + if(models.size() == 0 || models.size() != objectFiles.size()){return false;} + + for(unsigned int m = 0; m < models.size(); m++){ + + std::string object = sweep_folder+objectFiles[m].toStdString(); printf("object: %s\n",object.c_str()); + + reglib::Model * model = models[m]; + unsigned int current_displayInd = 0; + unsigned int maxNrPixels = 0; + for(unsigned int j = 0; j < model->frames.size(); j++){ + unsigned int nrPixels = model->modelmasks[j]->testw.size(); + + } + + + + printf("current_displayInd: %i\n",current_displayInd); + model->fullDelete(); + delete model; } +*/ + return true; } bool annotateFiles(std::string path){ diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index e0eb48c2..d3ca5296 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -292,7 +292,7 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); massregmod->timeout = 1200; massregmod->viewer = viewer; - massregmod->visualizationLvl = 0; + massregmod->visualizationLvl = 1; massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massregmod->nomask = true; diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index ad67efc0..eab06ac8 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -132,6 +132,8 @@ std::string posepath = "testposes.xml"; std::vector sweepPoses; reglib::Camera * basecam; +bool recomputeRelativePoses = false; + bool testDynamicObjectServiceCallback(std::string path); bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res); @@ -338,6 +340,32 @@ Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader){ return regpose; } +int readNumberOfViews(std::string xmlFile){ + QFile file(xmlFile.c_str()); + if (!file.exists()){return -1;} + + file.open(QIODevice::ReadOnly); + QXmlStreamReader* xmlReader = new QXmlStreamReader(&file); + + + int count = 0; + while (!xmlReader->atEnd() && !xmlReader->hasError()){ + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()){return -1;} + + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "View"){ + count++; + } + } + } + delete xmlReader; + return count; +} + void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses){ QFile file(xmlFile.c_str()); @@ -457,7 +485,7 @@ void readViewXML(std::string xmlFile, std::vector & frames, token = xmlReader->readNext();//Pose elementName = xmlReader->name().toString(); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, pose); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, regpose); frames.push_back(frame); poses.push_back(pose); } @@ -489,6 +517,11 @@ void setBaseSweep(std::string path){ } reglib::Model * processAV(std::string path){ + +// std::vector frames; +// std::vector poses; +// readViewXML(sweep_folder+"ViewGroup.xml",frames,poses); + printf("processAV: %s\n",path.c_str()); int slash_pos = path.find_last_of("/"); @@ -568,97 +601,7 @@ reglib::Model * processAV(std::string path){ reglib::Camera * cam = sweep->frames.front()->camera->clone(); reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m);//a.matrix()); frames.push_back(frame); -/* - //Visualize edges... - viewer->removeAllPointClouds(); - viewer->removeAllShapes(); - std::vector< std::vector > probs = frame->getImageProbs(); - unsigned char * det_dilatedata = frame->det_dilate.data; - - // printf("saving pcd: %s\n",path.c_str()); - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - unsigned short * depthdata = (unsigned short *)(frame->depth.data); - - const unsigned int width = frame->camera->width; - const unsigned int height = frame->camera->height; - const double idepth = frame->camera->idepth_scale; - const double cx = frame->camera->cx; - const double cy = frame->camera->cy; - const double ifx = 1.0/frame->camera->fx; - const double ify = 1.0/frame->camera->fy; - - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - cloud->width = width; - cloud->height = height; - cloud->points.resize(width*height); - - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - double z = idepth*double(depthdata[ind]); - - if(z > 0){ - pcl::PointXYZRGB p; - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - p.b = 0;//rgbdata[3*ind+0]; - p.g = 0;//rgbdata[3*ind+1]; - p.r = 0;//rgbdata[3*ind+2]; -// if(det_dilatedata[ind]){ -// p.r = 255; -// p.g = 255; -// p.b = 255; -// } - cloud->points[ind] = p; - } - } - } - - - for(unsigned int w = 1; w < width; w++){ - for(unsigned int h = 1; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGB & p = cloud->points[ind]; - - float px = 1.0-probs[0][ind]; - float py = 1.0-probs[1][ind]; - - p.r = 255.0*(1-px); - p.g = 255.0*(1-py); - - if(det_dilatedata[ind]){ - - - char buf [1024]; - { - pcl::PointXYZRGB p1 = cloud->points[ind-1]; - double d = sqrt(pow(p.x-p1.x,2)+pow(p.y-p1.y,2)+pow(p.z-p1.z,2)); - if(((p1.z < 1.5 && p1.z > 0) && (p.z < 1.5 && p.z > 0)) && d > 0.05){// ){//){ - //printf("myset.insert(%i);\n",ind); - sprintf(buf,"lineX_%i",ind); - viewer->addLine(p,p1,1-px,0,0,buf); - } - } - - { - pcl::PointXYZRGB p1 = cloud->points[ind-width]; - double d = sqrt(pow(p.x-p1.x,2)+pow(p.y-p1.y,2)+pow(p.z-p1.z,2)); - if(((p1.z < 1.5 && p1.z > 0) && (p.z < 1.5 && p.z > 0)) && d > 0.5){// ){//){ - //printf("myset.insert(%i);\n",ind); - sprintf(buf,"lineY_%i",ind); - viewer->addLine(p,p1,0,1-py,0,buf); - } - } - } - } - } - - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); -*/ - //both_unrefined.push_back(sweep->frames.front()->pose.inverse()*a.matrix()); both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); } @@ -668,7 +611,6 @@ reglib::Model * processAV(std::string path){ mu->massreg_timeout = 60*4; mu->viewer = viewer; - //sweep->points = mu->getSuperPoints(sweep->relativeposes,sweep->frames,sweep->modelmasks,1,false); sweep->recomputeModelPoints(); reglib::Model * fullmodel = new reglib::Model(); @@ -682,7 +624,7 @@ reglib::Model * processAV(std::string path){ bgmassreg->viewer = viewer; bgmassreg->use_surface = true; bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = 0;//visualization_lvl; + bgmassreg->visualizationLvl = visualization_lvl; bgmassreg->maskstep = 5; bgmassreg->nomaskstep = 5; bgmassreg->nomask = true; @@ -710,9 +652,7 @@ reglib::Model * processAV(std::string path){ // reglib::MassFusionResults bgmfr2 = bgmassreg2->getTransforms(bgmfr.poses); // delete bgmassreg2; - for(unsigned int i = 0; i < frames.size(); i++){ - frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1]; - } + for(unsigned int i = 0; i < frames.size(); i++){frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1];} for(unsigned int i = 0; i < frames.size(); i++){ fullmodel->frames.push_back(frames[i]); @@ -727,7 +667,7 @@ reglib::Model * processAV(std::string path){ delete reg; delete mu; delete sweep; - delete basecam; + //delete basecam; return fullmodel; } @@ -806,19 +746,49 @@ int processMetaroom(std::string path, bool store_old_xml = true){ int returnval = 0; printf("processing: %s\n",path.c_str()); - // testDynamicObjectServiceCallback(path); - // exit(0); - if ( ! boost::filesystem::exists( path ) ){return 0;} int slash_pos = path.find_last_of("/"); std::string sweep_folder = path.substr(0, slash_pos) + "/"; - reglib::Model * fullmodel = processAV(path); + int viewgroup_nrviews = readNumberOfViews(sweep_folder+"ViewGroup.xml"); - //savePoses(sweep_folder+"testposes.xml",fullmodel->relativeposes,17); - writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); + int additional_nrviews = 0; + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); + for (auto objectFile : objectFiles){ + auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); + additional_nrviews += object.vAdditionalViews.size(); + } + + SimpleXMLParser parser; + SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); + int metaroom_nrviews = current_roomData.vIntermediateRoomClouds.size(); + + + printf("viewgroup_nrviews: %i\n",viewgroup_nrviews); + printf("additional_nrviews: %i\n",additional_nrviews); + printf("metaroom_nrviews: %i\n",metaroom_nrviews); + + reglib::Model * fullmodel; + if(viewgroup_nrviews == (additional_nrviews+metaroom_nrviews) && !recomputeRelativePoses){ + printf("time to read old\n"); + fullmodel = new reglib::Model(); + + for(unsigned int i = 0; i < viewgroup_nrviews; i++){ + cv::Mat fullmask; + fullmask.create(480,640,CV_8UC1); + unsigned char * maskdata = (unsigned char *)fullmask.data; + for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} + fullmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); + } + + readViewXML(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); + fullmodel->recomputeModelPoints(); + }else{ + fullmodel = processAV(path); + writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); + } reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( fullmodel, reg); @@ -833,8 +803,8 @@ int processMetaroom(std::string path, bool store_old_xml = true){ //fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); //fullmodel->recomputeModelPoints(); - SimpleXMLParser parser; - SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); + + // SemanticRoom observation = SemanticRoomXMLParser::loadRoomFromXML(path,false); // Eigen::Matrix4f roomTransform = observation.getRoomTransform(); @@ -1528,6 +1498,7 @@ int main(int argc, char** argv){ else if(std::string(argv[i]).compare("-baseSweep") == 0){ inputstate = 16;} else if(std::string(argv[i]).compare("-once") == 0){ once = true;} else if(std::string(argv[i]).compare("-nobase") == 0){ baseSetting = false;} + else if(std::string(argv[i]).compare("-recomputeRelativePoses") == 0){ recomputeRelativePoses = true;} else if(inputstate == 0){ segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); }else if(inputstate == 1){ From a6c133fa8a14e6d9149d52ba4dd5cf0d21e7f59e Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 19 Oct 2016 23:31:37 +0200 Subject: [PATCH 169/255] .... --- .../metaroom_additional_view_processing.cpp | 28 +++++++++++++++---- 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index eab06ac8..cc363c96 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -518,9 +518,9 @@ void setBaseSweep(std::string path){ reglib::Model * processAV(std::string path){ -// std::vector frames; -// std::vector poses; -// readViewXML(sweep_folder+"ViewGroup.xml",frames,poses); + // std::vector frames; + // std::vector poses; + // readViewXML(sweep_folder+"ViewGroup.xml",frames,poses); printf("processAV: %s\n",path.c_str()); @@ -742,8 +742,7 @@ std::vector readPoseXML(std::string xmlFile){ return poses; } -int processMetaroom(std::string path, bool store_old_xml = true){ - int returnval = 0; +reglib::Model * getAVMetaroom(std::string path){ printf("processing: %s\n",path.c_str()); if ( ! boost::filesystem::exists( path ) ){return 0;} @@ -789,6 +788,23 @@ int processMetaroom(std::string path, bool store_old_xml = true){ fullmodel = processAV(path); writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); } + return fullmodel; +} + +int processMetaroom(std::string path, bool store_old_xml = true){ + int returnval = 0; + printf("processing: %s\n",path.c_str()); + + if ( ! boost::filesystem::exists( path ) ){return 0;} + + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + + SimpleXMLParser parser; + SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); + + reglib::Model * fullmodel = getAVMetaroom(path); reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( fullmodel, reg); @@ -832,7 +848,7 @@ int processMetaroom(std::string path, bool store_old_xml = true){ std::string prev = sweep_xmls[prevind]; printf("prev: %s\n",prev.c_str()); - reglib::Model * bg = quasimodo_brain::load_metaroom_model(prev); + reglib::Model * bg = getAVMetaroom(prev);//quasimodo_brain::load_metaroom_model(prev); auto sweep = SimpleXMLParser::loadRoomFromXML(prev, std::vector{},false); //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); //bg->recomputeModelPoints(); From aca502a3176c92d98a892c7b202544bf34c4d946 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 21 Oct 2016 23:15:14 +0200 Subject: [PATCH 170/255] ... --- .../quasimodo_brain/src/AnnotationTool.cpp | 122 ++++- quasimodo/quasimodo_brain/src/Util/Util.cpp | 137 ++++- quasimodo/quasimodo_brain/src/Util/Util.h | 4 +- .../metaroom_additional_view_processing.cpp | 60 +- .../src/segmentationserver.cpp | 6 +- .../quasimodo_brain/src/test_segment.cpp | 6 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 25 +- .../src/modelupdater/ModelUpdater.cpp | 512 +++--------------- 8 files changed, 379 insertions(+), 493 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp index 74120b23..10396ec3 100644 --- a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp +++ b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp @@ -89,6 +89,9 @@ #include +#include +#include + std::vector< ros::ServiceServer > m_DynamicObjectsServiceServers; std::vector< ros::ServiceServer > m_GetDynamicObjectServiceServers; @@ -161,7 +164,6 @@ Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader){ } void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses){ - QFile file(xmlFile.c_str()); if (!file.exists()) @@ -189,7 +191,7 @@ void readViewXML(std::string xmlFile, std::vector & frames, if (xmlReader->hasError()) { - ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + ROS_ERROR("XML error: %s in %s",xmlReader->errorString().toStdString().c_str(),xmlFile.c_str()); return; } @@ -309,7 +311,7 @@ std::vector readPoseXML(std::string xmlFile){ if (xmlReader->hasError()) { - ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + ROS_ERROR("XML error: %s in %s",xmlReader->errorString().toStdString().c_str(),xmlFile.c_str()); return poses; } @@ -369,7 +371,7 @@ std::vector loadModels(std::string path){ continue; if (xmlReader->hasError()){ - ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + ROS_ERROR("XML error: %s in %s",xmlReader->errorString().toStdString().c_str(),object.c_str()); break; } @@ -441,7 +443,7 @@ bool annotate(std::string path){ continue; if (xmlReader->hasError()){ - ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + ROS_ERROR("XML error: %s in %s",xmlReader->errorString().toStdString().c_str(), (roomFolder+"/ViewGroup.xml").c_str()); return false; } @@ -476,6 +478,11 @@ bool annotate(std::string path){ unsigned int current_displayInd = 0; unsigned int maxNrPixels = 0; + std::string classname = ""; + std::string instancename = ""; + std::string tags = ""; + + QFile objfile(object.c_str()); if (!objfile.exists()){ @@ -494,16 +501,22 @@ bool annotate(std::string path){ continue; if (objxmlReader->hasError()){ - ROS_ERROR("XML error: %s",objxmlReader->errorString().toStdString().c_str()); + ROS_ERROR("XML error: %s in %s",objxmlReader->errorString().toStdString().c_str(),object.c_str()); break; } QString elementName = objxmlReader->name().toString(); if (token == QXmlStreamReader::StartElement){ + if (objxmlReader->name() == "Object"){ + QXmlStreamAttributes attributes = objxmlReader->attributes(); + if (attributes.hasAttribute("classname")){ classname = attributes.value("classname").toString().toStdString();} + if (attributes.hasAttribute("instancename")){ instancename = attributes.value("instancename").toString().toStdString();} + if (attributes.hasAttribute("tags")){ tags = attributes.value("tags").toString().toStdString();} + } + if (objxmlReader->name() == "Mask"){ int number = 0; -// cv::Mat mask; QXmlStreamAttributes attributes = objxmlReader->attributes(); if (attributes.hasAttribute("filename")){ QString maskpath = attributes.value("filename").toString(); @@ -533,8 +546,7 @@ bool annotate(std::string path){ if(objectMasks.size() != imgNumber.size()){return false;} - std::string classname = ""; - std::string instancename = ""; + int fontFace = cv::FONT_HERSHEY_COMPLEX_SMALL; double fontScale = 1; int thickness = 1; @@ -557,7 +569,7 @@ bool annotate(std::string path){ unsigned int height = rgb.rows; unsigned int width = rgb.cols; - unsigned int newwidth = width+400; + unsigned int newwidth = width+600; unsigned char * rgbdata = rgb.data; @@ -568,7 +580,6 @@ bool annotate(std::string path){ for(unsigned int i = 0; i < 3*height*newwidth; i++){imgdata[i] = 0;} - for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ int oind = h*width+w; @@ -579,33 +590,36 @@ bool annotate(std::string path){ } } - int textnr = 0; - putText(img, " Class: "+classname, cv::Point(width+10, 30+(textnr++)*25 ), fontFace, fontScale, cv::Scalar::all(255), thickness, 8); - putText(img, " Instance: "+instancename, cv::Point(width+10, 30+(textnr++)*25 ), fontFace, fontScale, cv::Scalar::all(255), thickness, 8); - + putText(img, " Class: "+classname, cv::Point(width+10, 30+(textnr++)*25 ), fontFace, fontScale, cv::Scalar::all(255), thickness, 8); + putText(img, " Instance: "+instancename, cv::Point(width+10, 30+(textnr++)*25 ), fontFace, fontScale, cv::Scalar::all(255), thickness, 8); + putText(img, " Tags: "+tags, cv::Point(width+10, 30+(textnr++)*25 ), fontFace, fontScale, cv::Scalar::all(255), thickness, 8); char buf [1024]; sprintf(buf," Image: %i / %i",current_displayInd+1,objectMasks.size()); putText(img, std::string(buf), cv::Point(width+10, 30+(textnr++)*25 ), fontFace, fontScale, cv::Scalar::all(255), thickness, 8); + putText(img, " Select state(pushed CTRL)",cv::Point(width+10, 30+(textnr++)*25 ), fontFace, fontScale, cv::Scalar::all(255), thickness, 8); putText(img, "--", cv::Point(width+5, 30+state*25 ), fontFace, fontScale, cv::Scalar(0,255,0), thickness, 8); - cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb",img ); + cv::namedWindow( "Annotation tool", cv::WINDOW_AUTOSIZE ); cv::imshow( "Annotation tool",img ); char c = cv::waitKey(0); - printf("c: %c -> %i\n",c,int(c)); if(c == -29){ - state = 3; + state = 4; }else if(state == 0){ - if((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '1')){classname += c;} + if((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '9') || c == ' '){classname += c;} if(c == 8 && classname.size() > 0){classname.pop_back();} if(c == 10){state = 1;} }else if(state == 1){ - if((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '1')){instancename += c;} + if((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '9') || c == ' '){instancename += c;} if(c == 8 && instancename.size() > 0){instancename.pop_back();} - if(c == 10){break;} + if(c == 10){state = 2;} }else if(state == 2){ + if((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '9') || c == ' '){tags += c;} + if(c == 8 && tags.size() > 0){tags.pop_back();} + if(c == 10){break;} + }else if(state == 3){ if(c == 'q' || c == 'Q'){ current_displayInd = std::max(int(current_displayInd-1),0); rgb = cv::imread(rgbpaths[imgNumber[current_displayInd]], CV_LOAD_IMAGE_UNCHANGED); @@ -617,17 +631,77 @@ bool annotate(std::string path){ mask = objectMasks[current_displayInd]; } if(c == 10){state = 0;} - }else if(state == 3){ + }else if(state == 4){ if(c == '1'){state = 0;} if(c == '2'){state = 1;} if(c == '3'){state = 2;} + if(c == '4'){state = 3;} + if(c == 10){ state = 0;} } - } + delete objxmlReader; - delete objxmlReader; + std::streampos size; + char * memblock; + + std::ifstream file (object, ios::in|ios::binary|ios::ate); + if (file.is_open()){ + size = file.tellg(); + memblock = new char [size]; + file.seekg (0, ios::beg); + file.read (memblock, size); + file.close(); + + std::string filedata = std::string(memblock); + + //",found1b+1); + printf("found: %i\n",found1); + + + std::string frontpart = filedata.substr(0,found1b+1); + + std::string endpart = filedata.substr(found2,filedata.size()-found2-1); + +// std::cout << frontpart << std::endl; +// std::cout << endpart << std::endl; + +// if (attributes.hasAttribute("classname")){ classname = attributes.value("classname").toString().toStdString();} +// if (attributes.hasAttribute("instancename")){ instancename = attributes.value("instancename").toString().toStdString();} +// if (attributes.hasAttribute("tags")){ tags = attributes.value("tags").toString().toStdString();} + + + std::string total = frontpart+" classname=\""+classname+"\""+" instancename=\""+instancename+"\""+" tags=\""+tags+"\""+endpart; + + std::ofstream myfile; + myfile.open (object); + myfile << total; + myfile.close(); + + //std::cout << std::endl << std::endl << std::endl << total << std::endl; +//exit(0); +// while (found!=std::string::npos){ +// path.replace(found,2,"/"); +// found = path.find("//"); +// } + + std::cout << "the entire file content is in memory" << std::endl; + std::cout << total << std::endl; + + delete[] memblock; + }else{ + std::cout << "Unable to open file\n"; + } +// return 0; } /* diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index d3ca5296..5594fac4 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -2,6 +2,22 @@ namespace quasimodo_brain { +void cleanPath(std::string & path){ + std::size_t found = path.find("//"); + while (found!=std::string::npos){ + path.replace(found,2,"/"); + found = path.find("//"); + } +} + +bool xmlSortFunction (std::string & a, std::string & b) { + return true; +} + +void sortXMLs(std::vector & sweeps){ + +} + reglib::Model * loadFromRaresFormat(std::string path){ reglib::Model * model = new reglib::Model(); reglib::Model * sweep = load_metaroom_model(path); @@ -279,7 +295,124 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ return sweepmodel; } -void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ +void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ + double startTime = getTime(); + printf("running segment method\n"); + boost::shared_ptr viewer; + if(debugg){ + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); + viewer->addCoordinateSystem(0.01); + viewer->setBackgroundColor(1.0,1.0,1.0); + } + + reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); + massregmod->timeout = 1200; + massregmod->viewer = viewer; + massregmod->visualizationLvl = 0; + massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massregmod->nomask = true; + massregmod->stopval = 0.0001; + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( models.front(), reg); + mu->occlusion_penalty = 15; + mu->massreg_timeout = 60*4; + mu->viewer = viewer; + printf("total segment part1 time: %5.5fs\n",getTime()-startTime); + + std::vector cpmod; + if(models.size() > 0 && bgs.size() > 0){ + for(unsigned int i = 0; i < bgs.size(); i++){ + if(bgs[i]->points.size() == 0){bgs[i]->recomputeModelPoints();} + cpmod.push_back(bgs.front()->frames.front()->pose.inverse() * bgs[i]->frames.front()->pose); + massregmod->addModel(bgs[i]); + } + + for(int j = 0; j < models.size(); j++){ + if(models[j]->points.size() == 0){models[j]->recomputeModelPoints();} + cpmod.push_back(bgs.front()->frames.front()->pose.inverse() * models[j]->frames.front()->pose);//bg->relativeposes.front().inverse() * models[j]->relativeposes.front()); + massregmod->addModel(models[j]); + } + printf("total segment part1b time: %5.5fs\n",getTime()-startTime); + reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); + + for(unsigned int i = 0; i < bgs.size(); i++){cpmod[i] = mfrmod.poses[i];} + + for(int j = 0; j < models.size(); j++){ + Eigen::Matrix4d change = mfrmod.poses[j+bgs.size()]; + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + } + for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ + models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; + } + } + }else if(models.size() > 1){ + for(int j = 0; j < models.size(); j++){ + if(models[j]->points.size() == 0){models[j]->recomputeModelPoints();} + cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); + massregmod->addModel(models[j]); + } + reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); + for(int j = 0; j < models.size(); j++){ + Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); + + for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ + models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; + } + + for(unsigned int k = 0; k < models[j]->submodels_relativeposes.size(); k++){ + models[j]->submodels_relativeposes[k] = change*models[j]->submodels_relativeposes[k]; + } + } + } + delete massregmod; + + printf("total segment part2 time: %5.5fs\n",getTime()-startTime); + + std::vector bgcp; + std::vector bgcf; + std::vector bgmask; + for(unsigned int i = 0; i < bgs.size(); i++){ + Eigen::Matrix4d cp = cpmod[i]; + std::cout << cp << std::endl; + for(unsigned int k = 0; k < bgs[i]->relativeposes.size(); k++){ + bgcp.push_back(cp * bgs[i]->relativeposes[k]); + bgcf.push_back(bgs[i]->frames[k]); + bgmask.push_back(bgs[i]->modelmasks[k]->getMask()); + } + } + for(int j = 0; j < models.size(); j++){ + reglib::Model * model = models[j]; + + std::vector masks; + for(unsigned int i = 0; i < model->frames.size(); i++){ + reglib::RGBDFrame * frame = model->frames[i]; + reglib::Camera * cam = frame->camera; + cv::Mat mask; + mask.create(cam->height,cam->width,CV_8UC1); + unsigned char * maskdata = (unsigned char *)(mask.data); + for(unsigned int k = 0; k < cam->height*cam->width;k++){maskdata[k] = 255;} + masks.push_back(mask); + } + + std::vector movemask; + std::vector dynmask; + printf("computeMovingDynamicStatic\n"); + mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions + external.push_back(movemask); + internal.push_back(masks); + dynamic.push_back(dynmask); + } + + delete reg; + delete mu; + printf("total segment time: %5.5fs\n",getTime()-startTime); +} + +/* +void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ double startTime = getTime(); printf("running segment method\n"); boost::shared_ptr viewer; @@ -386,7 +519,7 @@ void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vec delete reg; delete mu; printf("total segment time: %5.5fs\n",getTime()-startTime); - //exit(0); } +*/ } diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index f4fe86b4..f471776d 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -38,6 +38,8 @@ #include namespace quasimodo_brain { +void cleanPath(std::string & path); +void sortXMLs(std::vector & sweeps); std::vector getRegisteredViewPosesFromFile(std::string poses_file, int no_transforms); reglib::Model * loadFromRaresFormat(std::string path); @@ -53,7 +55,7 @@ std::vector getRegisteredViewPoses(const std::string& poses_fil Eigen::Matrix4d getMat(tf::StampedTransform tf); reglib::Model * load_metaroom_model(std::string sweep_xml); -void segment(reglib::Model * bg, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg = false); +void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg = false); std::vector loadModelsXML(std::string path); } diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index cc363c96..3582a3ba 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -753,9 +753,12 @@ reglib::Model * getAVMetaroom(std::string path){ int viewgroup_nrviews = readNumberOfViews(sweep_folder+"ViewGroup.xml"); + printf("sweep_folder: %s\n",sweep_folder.c_str()); + int additional_nrviews = 0; QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); for (auto objectFile : objectFiles){ + printf("objectFile: %s\n",(sweep_folder+objectFile.toStdString()).c_str()); auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); additional_nrviews += object.vAdditionalViews.size(); } @@ -788,10 +791,14 @@ reglib::Model * getAVMetaroom(std::string path){ fullmodel = processAV(path); writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); } + + return fullmodel; } int processMetaroom(std::string path, bool store_old_xml = true){ + + quasimodo_brain::cleanPath(path); int returnval = 0; printf("processing: %s\n",path.c_str()); @@ -800,6 +807,8 @@ int processMetaroom(std::string path, bool store_old_xml = true){ int slash_pos = path.find_last_of("/"); std::string sweep_folder = path.substr(0, slash_pos) + "/"; + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); + store_old_xml = objectFiles.size() == 0; SimpleXMLParser parser; SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); @@ -842,33 +851,39 @@ int processMetaroom(std::string path, bool store_old_xml = true){ if(sweep_xmls[i].compare(path) == 0){break;} if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} } + int nextind = prevind +2; + + std::vector< reglib::Model * > models; + models.push_back(fullmodel); + std::vector< std::vector< cv::Mat > > internal; + std::vector< std::vector< cv::Mat > > external; + std::vector< std::vector< cv::Mat > > dynamic; + std::vector< reglib::Model * > bgs; if(prevind != -1){ std::string prev = sweep_xmls[prevind]; printf("prev: %s\n",prev.c_str()); + reglib::Model * bg = getAVMetaroom(prev); + bgs.push_back(bg); + } - reglib::Model * bg = getAVMetaroom(prev);//quasimodo_brain::load_metaroom_model(prev); - auto sweep = SimpleXMLParser::loadRoomFromXML(prev, std::vector{},false); - //bg->points = mu->getSuperPoints(bg->relativeposes,bg->frames,bg->modelmasks,1,false); - //bg->recomputeModelPoints(); - - std::vector< reglib::Model * > models; - models.push_back(fullmodel); + if(nextind < sweep_xmls.size()){ + std::string next = sweep_xmls[nextind]; + printf("next: %s\n",next.c_str()); + reglib::Model * nxt = getAVMetaroom(next); + bgs.push_back(nxt); + } - std::vector< std::vector< cv::Mat > > internal; - std::vector< std::vector< cv::Mat > > external; - std::vector< std::vector< cv::Mat > > dynamic; + if(bgs.size() > 0){ + auto sweep = SimpleXMLParser::loadRoomFromXML(path, std::vector{},false); - quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization_lvl > 0); + quasimodo_brain::segment(bgs,models,internal,external,dynamic,visualization_lvl > 0); remove_old_seg(sweep_folder); - if(models.size() == 0){ - returnval = 2; - }else{ - returnval = 3; - } + if(models.size() == 0){ returnval = 2;} + else{ returnval = 3;} for(unsigned int i = 0; i < models.size(); i++){ std::vector internal_masks = internal[i]; @@ -1009,7 +1024,9 @@ int processMetaroom(std::string path, bool store_old_xml = true){ xmlWriter->writeStartDocument(); xmlWriter->writeStartElement("Object"); xmlWriter->writeAttribute("object_number", QString::number(dynamicCounter-1)); - xmlWriter->writeAttribute("label", QString("")); + xmlWriter->writeAttribute("classname", QString("")); + xmlWriter->writeAttribute("instancename", QString("")); + xmlWriter->writeAttribute("tags", QString("")); xmlWriter->writeStartElement("Mean"); xmlWriter->writeAttribute("x", QString::number(sumx/sum)); xmlWriter->writeAttribute("y", QString::number(sumy/sum)); @@ -1048,7 +1065,7 @@ int processMetaroom(std::string path, bool store_old_xml = true){ for(unsigned int j = 0; j < mod_fr.size(); j++){ reglib::RGBDFrame * frame = mod_fr[j]; //std::cout << mod_po[j] << std::endl; - Eigen::Matrix4d p = mod_po[j]*bg->frames.front()->pose; + Eigen::Matrix4d p = mod_po[j]*bgs.front()->frames.front()->pose; unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); unsigned short * depthdata = (unsigned short *)(frame->depth.data); float * normalsdata = (float *)(frame->normals.data); @@ -1142,12 +1159,15 @@ int processMetaroom(std::string path, bool store_old_xml = true){ }else{break;} } } - bg->fullDelete(); - delete bg; }else{ returnval = 1; } + for(unsigned int i = 0; i < bgs.size(); i++){ + bgs[i]->fullDelete(); + delete bgs[i]; + } + fullmodel->fullDelete(); delete fullmodel; // delete bgmassreg; diff --git a/quasimodo/quasimodo_brain/src/segmentationserver.cpp b/quasimodo/quasimodo_brain/src/segmentationserver.cpp index dd5366b7..03ad3461 100644 --- a/quasimodo/quasimodo_brain/src/segmentationserver.cpp +++ b/quasimodo/quasimodo_brain/src/segmentationserver.cpp @@ -52,7 +52,11 @@ bool segment_model(quasimodo_msgs::segment_model::Request & req, quasimodo_msgs std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; - quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization); + std::vector< reglib::Model * > bgs; + bgs.push_back(bg); + + //quasimodo_brain::segment(bg,models,internal,external,dynamic,visualization); + quasimodo_brain::segment(bgs,models,internal,external,dynamic,visualization); printf("visualization: %i\n",visualization); for(unsigned int i = 0; false && visualization && i < models.size(); i++){ diff --git a/quasimodo/quasimodo_brain/src/test_segment.cpp b/quasimodo/quasimodo_brain/src/test_segment.cpp index dbae5700..b39c9ed0 100644 --- a/quasimodo/quasimodo_brain/src/test_segment.cpp +++ b/quasimodo/quasimodo_brain/src/test_segment.cpp @@ -215,7 +215,11 @@ int main(int argc, char** argv){ std::vector< std::vector< cv::Mat > > internal; std::vector< std::vector< cv::Mat > > external; std::vector< std::vector< cv::Mat > > dynamic; - quasimodo_brain::segment(models[i-1],foreground,internal,external,dynamic,false); + + std::vector< reglib::Model * > background; + background.push_back(models[i-1]); + + quasimodo_brain::segment(background,foreground,internal,external,dynamic,false); // quasimodo_msgs::segment_model sm; // sm.request.models.push_back(quasimodo_brain::getModelMSG(models[i])); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 8752ec10..869a379e 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -999,11 +999,14 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt cenms.create(height,width,CV_32FC3); float * cenmsdata = (float*)cenms.data; for(int i = 0; i < nr_pixels; i++){ - double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]); - edgep = std::max(0.0001,edgep); - cenmsdata[3*i+0] = edgep; - cenmsdata[3*i+1] = edgep; - cenmsdata[3*i+2] = edgep; + double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]); + edgep = std::max(0.0001,edgep); + cenmsdata[3*i+0] = edgep; + cenmsdata[3*i+1] = edgep; + cenmsdata[3*i+2] = edgep; +// cenmsdata[3*i+0] = 0; +// cenmsdata[3*i+1] = std::max(0.0001f,cedata[3*i+1]*maxima_dxdata[i]); +// cenmsdata[3*i+2] = std::max(0.0001f,cedata[3*i+2]*maxima_dydata[i]); } // for(int i = 0; i < nr_pixels; i++){ @@ -1479,16 +1482,16 @@ std::vector< std::vector > RGBDFrame::getImageProbs(bool depthonly){ dyc.resize(nr_pixels); for(unsigned int i = 0; i < nr_pixels;i++){ - dxc[i] = dedata[3*i+1]; - dyc[i] = dedata[3*i+2]; + dxc[i] = 1.0-dedata[3*i+1]; + dyc[i] = 1.0-dedata[3*i+2]; if(!depthonly){ if(!det_dilatedata[i]){ - dxc[i] = std::max(dedata[3*i+1],cedata[3*i+1]); - dyc[i] = std::max(dedata[3*i+2],cedata[3*i+2]); + dxc[i] = 1.0-std::max(dedata[3*i+1],cedata[3*i+1]); + dyc[i] = 1.0-std::max(dedata[3*i+2],cedata[3*i+2]); }else{ - dxc[i] = std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); - dyc[i] = std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); + dxc[i] = 1.0-std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); + dyc[i] = 1.0-std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); } } } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index bd58c432..89ff3489 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2138,7 +2138,13 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & if(dst_detdata[dst_ind] != 0){continue;} if(src_detdata[src_ind] != 0){continue;} - overlaps[src_ind] = std::min(0.9,std::max(overlaps[src_ind],p_overlap)); + double olp = overlaps[src_ind]; + double nolp = 1-olp; + nolp *= (1-p_overlap); + //overlaps[src_ind] = std::min(0.9999,std::max(olp,p_overlap)); + overlaps[src_ind] = std::min(0.9999,1-nolp); + // + //overlaps[src_ind] = std::min(0.9999,std::max(overlaps[src_ind],p_overlap)); //double prev = 1.0-occlusions[src_ind]; occlusions[src_ind] += p_occlusion;//std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); notocclusions[src_ind]++; @@ -2321,8 +2327,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s int offset = offsets[i]; RGBDFrame * frame = frames[i]; startTime = getTime(); - //std::vector< std::vector > probs = frame->getImageProbs();//getImageProbs(frame,9); - std::vector< std::vector > probs = getImageProbs(frame,9); + std::vector< std::vector > probs = frame->getImageProbs();//getImageProbs(frame,9); + //std::vector< std::vector > probs2 = getImageProbs(frame,9); total_connectiontime += getTime()-startTime; pixel_weights.push_back(probs); @@ -2373,16 +2379,16 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } for(unsigned int j = 0; j < nr_pixels; j++){ - bg_occlusions[j] = std::min(0.99,bg_occlusions[j]/std::max(1.0,bg_notocclusions[j])); - current_occlusions[j] = std::min(0.99,current_occlusions[j]/std::max(1.0,current_notocclusions[j])); + bg_occlusions[j] = std::min(0.999,bg_occlusions[j]/std::max(1.0,bg_notocclusions[j])); + current_occlusions[j] = std::min(0.999,current_occlusions[j]/std::max(1.0,current_notocclusions[j])); //bg_occlusions[j] = bg_occlusions[j] /bg_notocclusions[j]; //current_occlusions[j] = current_occlusions[j] /current_notocclusions[j]; } -// for(unsigned int j = 0; j < nr_pixels; j++){ -// bg_occlusions[j] = bg_occlusions[j] /(bg_occlusions[j] +bg_notocclusions[j]); -// current_occlusions[j] = current_occlusions[j] /(current_occlusions[j] +current_notocclusions[j]); -// } + // for(unsigned int j = 0; j < nr_pixels; j++){ + // bg_occlusions[j] = bg_occlusions[j] /(bg_occlusions[j] +bg_notocclusions[j]); + // current_occlusions[j] = current_occlusions[j] /(current_occlusions[j] +current_notocclusions[j]); + // } total_Dynw += getTime()-startTime; cv::Mat priorsimg; @@ -2404,7 +2410,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s p_dynamic += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); p_moving += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); - float p_static = std::max(bg_overlaps[ind]-p_moving-p_dynamic,0.0); + float p_static = std::max(0.5*bg_overlaps[ind]-p_moving-p_dynamic,0.0); float leftover = 1-p_moving-p_dynamic-p_static; float p_dyn = p_dynamic + (0.5-bias)*leftover; @@ -2503,6 +2509,26 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s printf("local inference time: %10.10fs\n\n",getTime()-start_inf); + +// cv::Mat edgeimgX; +// edgeimgX.create(height,width,CV_32FC3); +// float * edgedataX = (float*)edgeimgX.data; + +// cv::Mat edgeimgY; +// edgeimgY.create(height,width,CV_32FC3); +// float * edgedataY = (float*)edgeimgY.data; + +// for(int j = 0; j < width*height; j++){ +// edgedataX[3*j+0] = 0; +// edgedataX[3*j+1] = 1-probs[0][j]; +// edgedataX[3*j+2] = probs2[0][j]; + +// edgedataY[3*j+0] = 0; +// edgedataY[3*j+1] = 1-probs[1][j]; +// edgedataY[3*j+2] = probs2[1][j]; +// } + + // cv::Mat labelimg; // labelimg.create(height,width,CV_32FC3); // float * labelimgdata = (float*)labelimg.data; @@ -2512,6 +2538,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s // labelimgdata[3*ind+1] = label; // labelimgdata[3*ind+2] = 1-label; // } +// cv::namedWindow( "edgeimgX" , cv::WINDOW_AUTOSIZE ); cv::imshow( "edgeimgX", edgeimgX ); +// cv::namedWindow( "edgeimgY" , cv::WINDOW_AUTOSIZE ); cv::imshow( "edgeimgY", edgeimgY ); // cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb ); // cv::namedWindow( "depth" , cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", frame->depth ); // cv::namedWindow( "priors" , cv::WINDOW_AUTOSIZE ); cv::imshow( "priors", priorsimg ); @@ -2594,7 +2622,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; - if(w > 0 && probs[0][ind] > 0.00000001){ + if(w > 0 && probs[0][ind] > 0.00000001 && w < width-1){ int other = ind-1; double p_same = std::min(double(probs[0][ind]),maxprob_same); double not_p_same = 1-p_same; @@ -2602,7 +2630,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s g -> add_edge( ind+offset, other+offset, weight, weight ); } - if(h > 0 && probs[1][ind] > 0.00000001){ + if(h > 0 && probs[1][ind] > 0.00000001 && h < height-1) { int other = ind-width; double p_same = std::min(double(probs[1][ind]),maxprob_same); double not_p_same = 1-p_same; @@ -2871,7 +2899,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s viewer->spin(); } - if(score < 0){continue;} if(current_label == 1){ labelID.back() = ++nr_obj_dyn; printf("Dynamic: %f -> %f\n",score,totsum); @@ -2893,426 +2920,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s printf("connectedComponent: %5.5fs\n",getTime()-start_inf); -/* - double tot_inf = 0; - for(unsigned int it = 0; it < 10; it++){ - double start_inf1 = getTime(); - - g -> maxflow(); - - double diffs = 0; - for(unsigned int i = 0; i < interframe_connectionId.size();i++){ - for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ - double weight = interframe_connectionStrength[i][j]; - unsigned long other = interframe_connectionId[i][j]; - if(weight > 1 && g->what_segment(i) != g->what_segment(other)){ - diffs++; - } - } - } - - if(diffs < 1000){break;} - - double adds = 100000; - double prob = std::min(adds / diffs,1.0); - printf("diffs: %f adds: %f prob: %f\n",diffs,adds,prob); - - double trueadds = 0; - - for(unsigned int i = 0; i < interframe_connectionId.size();i++){ - for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ - double weight = interframe_connectionStrength[i][j]; - unsigned long other = interframe_connectionId[i][j]; - if(weight > 0.1 && g->what_segment(i) != g->what_segment(other)){ - if(rand() <= prob*RAND_MAX){ - trueadds++; - g -> add_edge( i, other, weight, weight ); - - interframe_connectionId_added[i].push_back(other); - interframe_connectionStrength_added[i].push_back(weight); - - interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back(); - interframe_connectionStrength[i].pop_back(); - - interframe_connectionId[i][j] = interframe_connectionId[i].back(); - interframe_connectionId[i].pop_back(); - j--; - } - } - } - } - printf("trueadds: %f\n",trueadds); - - tot_inf += getTime()-start_inf1; - printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf); - - if(tot_inf > 90){break;} - } - - for(unsigned int i = 0; i < interframe_connectionId.size();i++){ - for(unsigned int j = 0; j < interframe_connectionId_added[i].size();j++){ - interframe_connectionStrength[i].push_back(interframe_connectionStrength_added[i][j]); - interframe_connectionId[i].push_back(interframe_connectionId_added[i][j]); - } - } - - int dynamic_label = 1; - //std::vector labels; - std::vector dyn_ind; - std::vector stat_ind; - //labels.resize(current_point); - stat_ind.resize(current_point); - long nr_dynamic = 0; - for(unsigned long i = 0; i < current_point;i++){ - labels[i] = g->what_segment(i); - nr_dynamic += labels[i]==dynamic_label; - if(labels[i]==dynamic_label){ - stat_ind[i] = dyn_ind.size(); - dyn_ind.push_back(i); - } - } - delete g; - - double end_inf = getTime(); - printf("static inference time: %10.10fs\n",end_inf-start_inf); - - long dynamic_frameConnections = 0; - - for(unsigned long i = 0; i < frames.size(); i++){ - int offset = offsets[i]; - Camera * camera = frames[i]->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - std::vector< std::vector > & probs = pixel_weights[i]; - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - if(labels[ind+offset] == dynamic_label){ - if(w > 0 && probs[0][ind] > 0.00000001){ - int other = ind-1; - if(labels[ind+offset] == labels[other+offset]){dynamic_frameConnections++;} - } - - if(h > 0 && probs[1][ind] > 0.00000001){ - int other = ind-width; - if(labels[ind+offset] == labels[other+offset]){dynamic_frameConnections++;} - } - } - } - } - } - - long dynamic_interframeConnections = 0; - for(unsigned long i = 0; i < current_point;i++){ - if(labels[i] == dynamic_label){ - for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ - if(labels[interframe_connectionId[i][j]] == labels[i]){dynamic_interframeConnections++;} - } - } - } - - start_inf = getTime(); - gc::Graph * dynamic_g = new gc::Graph(nr_dynamic,dynamic_frameConnections+dynamic_interframeConnections); - for(unsigned long i = 0; i < dyn_ind.size();i++){ - dynamic_g -> add_node(); - double p_fg = priors[3*dyn_ind[i]+0]; - double p_bg = priors[3*dyn_ind[i]+1]; - double norm = p_fg+p_bg; - p_fg /= norm; - p_bg /= norm; - if(norm <= 0){ - dynamic_g -> add_tweights( i, 0, 0 ); - continue; - } - - double weightFG = -log(p_fg); - double weightBG = -log(p_bg); - dynamic_g -> add_tweights( i, weightFG, weightBG ); - } - - for(unsigned long i = 0; i < frames.size(); i++){ - int offset = offsets[i]; - Camera * camera = frames[i]->camera; - const unsigned int width = camera->width; - const unsigned int height = camera->height; - std::vector< std::vector > & probs = pixel_weights[i]; - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - if(labels[ind+offset] == dynamic_label){ - if(w > 0 && probs[0][ind] > 0.00000001){ - int other = ind-1; - if(labels[ind+offset] == labels[other+offset]){ - //dynamic_frameConnections++; - double p_same = std::min(double(probs[0][ind]),maxprob_same); - double not_p_same = 1-p_same; - double weight = -log(not_p_same); - dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight ); - } - } - - if(h > 0 && probs[1][ind] > 0.00000001){ - int other = ind-width; - if(labels[ind+offset] == labels[other+offset]){ - //dynamic_frameConnections++; - double p_same = std::min(double(probs[0][ind]),maxprob_same); - double not_p_same = 1-p_same; - double weight = -log(not_p_same); - dynamic_g -> add_edge( stat_ind[ind+offset], stat_ind[other+offset], weight, weight ); - } - } - } - } - } - } - - for(unsigned long i = 0; i < current_point;i++){ - if(labels[i] == dynamic_label){ - for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ - if(labels[interframe_connectionId[i][j]] == labels[i]){ - double weight = interframe_connectionStrength[i][j]; - dynamic_g -> add_edge( stat_ind[i], stat_ind[ interframe_connectionId[i][j] ], weight, weight ); - } - } - } - } - dynamic_g -> maxflow(); - - - tot_inf = 0; - for(unsigned int it = 0; false && it < 100; it++){ - double start_inf1 = getTime(); - dynamic_g -> maxflow(); - - double diffs = 0; - for(unsigned long i = 0; i < current_point;i++){ - if(labels[i] == dynamic_label){ - for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ - if(labels[interframe_connectionId[i][j]] == labels[i]){ - double weight = interframe_connectionStrength[i][j]; - - unsigned long i2 = stat_ind[i]; - unsigned long j2 = stat_ind[ interframe_connectionId[i][j] ]; - if(weight > 1 && g->what_segment(i2) != g->what_segment(j2)){ - diffs++; - } - } - } - } - } - - - double adds = 100000; - double prob = std::min(adds / diffs,1.0); - printf("diffs: %f adds: %f prob: %f\n",diffs,adds,prob); - - if(diffs < 1000){break;} - - - double trueadds = 0; - - for(unsigned int i = 0; i < interframe_connectionId.size();i++){ - if(labels[i] == dynamic_label){ - for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ - if(labels[interframe_connectionId[i][j]] == labels[i]){ - - double weight = interframe_connectionStrength[i][j]; - unsigned long other = interframe_connectionId[i][j]; - - unsigned long i2 = stat_ind[i]; - unsigned long j2 = stat_ind[ interframe_connectionId[i][j] ]; - if(weight > 1 && g->what_segment(i2) != g->what_segment(j2)){ - if(rand() <= prob*RAND_MAX){ - trueadds++; - g -> add_edge( i2, j2, weight, weight ); - - interframe_connectionId_added[i].push_back(other); - interframe_connectionStrength_added[i].push_back(weight); - - interframe_connectionStrength[i][j] = interframe_connectionStrength[i].back(); - interframe_connectionStrength[i].pop_back(); - - interframe_connectionId[i][j] = interframe_connectionId[i].back(); - interframe_connectionId[i].pop_back(); - j--; - } - } - } - } - } - } - printf("trueadds: %f\n",trueadds); - - tot_inf += getTime()-start_inf1; - printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf); - if(tot_inf > 90){break;} - } - - end_inf = getTime(); - printf("dynamic inference time: %10.10fs\n",end_inf-start_inf); - - long nr_moving = 0; - for(unsigned long i = 0; i < dyn_ind.size();i++){ - int res = dynamic_g->what_segment(i); - if(res == 1){ - labels[dyn_ind[i]] = 2; - nr_moving++; - } - } - delete dynamic_g; - - for(unsigned int i = 0; i < interframe_connectionId.size();i++){ - for(unsigned int j = 0; j < interframe_connectionId_added[i].size();j++){ - interframe_connectionStrength[i].push_back(interframe_connectionStrength_added[i][j]); - interframe_connectionId[i].push_back(interframe_connectionId_added[i][j]); - } - } - - const unsigned int nr_frames = frames.size(); - const unsigned int width = frames[0]->camera->width; - const unsigned int height = frames[0]->camera->height; - const unsigned int pixels_per_image = width*height; - const unsigned int nr_pixels = nr_frames*pixels_per_image; - - double probthresh = 0.5; - double str_probthresh = -log(probthresh); - unsigned int number_of_dynamics = 0; - unsigned int nr_obj_dyn = 0; - unsigned int nr_obj_mov = 0; - std::vector objectlabel; - std::vector labelID; - labelID.push_back(0); - objectlabel.resize(nr_pixels); - for(unsigned long i = 0; i < nr_pixels; i++){objectlabel[i] = 0;} - for(unsigned long ind = 0; ind < nr_pixels; ind++){ - if(valids[ind] && objectlabel[ind] == 0 && labels[ind] != 0){ - unsigned int current_label = labels[ind]; - number_of_dynamics++; - objectlabel[ind] = number_of_dynamics; - unsigned long todocounter = 0; - std::vector< unsigned long > todo; - todo.push_back(ind); - double score = 0; - double totsum = 0; - while(todocounter < todo.size()){ - unsigned long cind = todo[todocounter++]; - unsigned long frameind = cind / pixels_per_image; - - - unsigned long iind = cind % pixels_per_image; - unsigned long w = iind % width; - unsigned long h = iind / width; - - double p0 = priors[3*cind+0]; - double p1 = priors[3*cind+1]; - double p2 = priors[3*cind+2]; - - if(valids[cind]){ - double s = 0; - if(p0 > p2){s += p1 - p0;} - else{ s += p1 - p2;} - score += s; - totsum++; - } - - float * dedata = (float*)(frames[frameind]->de.data); - - int dir; - dir = -1; - if( w > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ - objectlabel[cind+dir] = number_of_dynamics; - todo.push_back(cind+dir); - } - - dir = 1; - if( w < (width-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ - objectlabel[cind+dir] = number_of_dynamics; - todo.push_back(cind+dir); - } - - dir = -width; - if( h > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ - objectlabel[cind+dir] = number_of_dynamics; - todo.push_back(cind+dir); - } - - dir = width; - if( h < (height-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ - objectlabel[cind+dir] = number_of_dynamics; - todo.push_back(cind+dir); - } - - for(unsigned long j = 0; j < interframe_connectionId[cind].size();j++){ - unsigned long other = interframe_connectionId[cind][j]; - if(interframe_connectionStrength[cind][j] > str_probthresh && objectlabel[other] == 0 && labels[other] == current_label){ - objectlabel[other] = number_of_dynamics; - todo.push_back(other); - } - } - } - - printf("score: %f\n",score); - - - labelID.push_back(0); - if(score < 200){continue;} - - if(debugg && todo.size() > 10000){ - printf("3-class\n"); - for(unsigned int i = 0; i < current_point; i++){ - cloud->points[i].r = priors[3*i+0]*255.0; - cloud->points[i].g = priors[3*i+1]*255.0; - cloud->points[i].b = priors[3*i+2]*255.0; - } - - pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); - cloud_sample->points.clear(); - for(unsigned int i = 0; i < current_point; i++){ - cloud_sample->points.push_back(cloud->points[i]); - } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); - viewer->spin(); - - for(unsigned int i = 0; i < current_point; i++){ - cloud_sample->points[i].r = 255; - cloud_sample->points[i].g = 0; - cloud_sample->points[i].b = 0; - } - - for(unsigned int j = 0; j < todo.size(); j++){ - unsigned int i = todo[j]; - cloud_sample->points[i].r = 0; - cloud_sample->points[i].g = 255; - cloud_sample->points[i].b = 0; - } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); - viewer->spin(); - } - - if(score < 0){continue;} - if(current_label == 1){ - labelID.back() = ++nr_obj_dyn; - printf("Dynamic: %f -> %f\n",score,totsum); - sr.component_dynamic.push_back(todo); - sr.scores_dynamic.push_back(score); - sr.total_dynamic.push_back(totsum); - } - if(current_label == 2){ - labelID.back() = --nr_obj_mov; - printf("Moving: %f -> %f\n",score,totsum); - sr.component_moving.push_back(todo); - sr.scores_moving.push_back(score); - sr.total_moving.push_back(totsum); - } - } - } - interframe_connectionId.clear(); - interframe_connectionStrength.clear(); - - printf("connectedComponent: %5.5fs\n",getTime()-start_inf); -*/ int current = 0; for(unsigned long i = 0; i < frames.size(); i++){ Camera * camera = frames[i]->camera; @@ -3331,6 +2938,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s d2.create(height,width,CV_8UC1); unsigned char * ddata2 = (unsigned char*)d2.data; + for(int j = 0; j < width*height; j++){ mdata[j] = 0; ddata[j] = 0; @@ -3346,6 +2954,44 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } movemask.push_back(m); dynmask.push_back(d); + + +// unsigned short * depthdata = (unsigned short *)(frames[i]->depth.data); +// cv::Mat debuggimg; +// debuggimg.create(height,width,CV_8UC3); +// unsigned char * debuggimgdata = (unsigned char*)debuggimg.data; +// cv::Mat edgeimg; +// edgeimg.create(height,width,CV_32FC3); +// float * edgedata = (float*)edgeimg.data; +// for(unsigned int label = 0; label < labelID.size(); label++){ +// int lid = labelID[label]; +// printf("lid: %i\n",lid); +// if(lid <= 0){continue;} +// bool hasdata = false; +// for(int j = 0; j < width*height; j++){ +// if(ddata[j] == lid){ +// debuggimgdata[3*j+0] = 255; +// debuggimgdata[3*j+1] = 255; +// debuggimgdata[3*j+2] = 255; +// hasdata = true; +// }else{ +// debuggimgdata[3*j+0] = 0; +// debuggimgdata[3*j+1] = 0; +// debuggimgdata[3*j+2] = 0; +// } +// if(depthdata[j] == 0){ +// debuggimgdata[3*j+0] = 255; +// debuggimgdata[3*j+1] = 0; +// debuggimgdata[3*j+2] = 255; +// } +// } +// if(hasdata){ +// cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frames[i]->rgb ); +// cv::namedWindow( "debuggimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "debuggimg", debuggimg ); +// cv::namedWindow( "edgeimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "edgeimg", edgeimg ); +// cv::waitKey(0); +// } +// } } if(debugg){ From 604a7a4433537d70e483b22f5da88ca5ba21861d Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 24 Oct 2016 16:38:08 +0200 Subject: [PATCH 171/255] ... --- .../quasimodo_brain/src/AnnotationTool.cpp | 16 ++-- .../metaroom_additional_view_processing.cpp | 74 +++++++++++++++---- .../registration/RegistrationRefinement.cpp | 4 +- 3 files changed, 71 insertions(+), 23 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp index 10396ec3..b0b143ff 100644 --- a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp +++ b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp @@ -744,12 +744,8 @@ bool annotateFiles(std::string path){ } int main(int argc, char** argv){ - visualization_lvl = 1; + visualization_lvl = 0; - viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0.5, 0, 0.5); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); std::vector< std::string > folders; int inputstate = 0; @@ -757,10 +753,20 @@ int main(int argc, char** argv){ printf("input: %s\n",argv[i]); if(std::string(argv[i]).compare("-v") == 0){ inputstate = 1;} else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 2;} + else if(std::string(argv[i]).compare("-folders") == 0){ inputstate = 2;} + else if(std::string(argv[i]).compare("-file") == 0){ inputstate = 2;} + else if(std::string(argv[i]).compare("-files") == 0){ inputstate = 2;} else if(inputstate == 1){visualization_lvl = atoi(argv[i]);} else if(inputstate == 2){folders.push_back(std::string(argv[i]));} } + if(visualization_lvl > 0){ + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + } + if(folders.size() == 0){folders.push_back(std::string(getenv ("HOME"))+"/.semanticMap/");} for(unsigned int i = 0; i < folders.size(); i++){annotateFiles(folders[i]);} diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 3582a3ba..76ca9767 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -517,11 +517,6 @@ void setBaseSweep(std::string path){ } reglib::Model * processAV(std::string path){ - - // std::vector frames; - // std::vector poses; - // readViewXML(sweep_folder+"ViewGroup.xml",frames,poses); - printf("processAV: %s\n",path.c_str()); int slash_pos = path.find_last_of("/"); @@ -619,12 +614,42 @@ reglib::Model * processAV(std::string path){ fullmodel->modelmasks = sweep->modelmasks; if(frames.size() > 0){ - reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.15); + + reglib::RegistrationRefinement * refinement = new reglib::RegistrationRefinement(); + refinement->viewer = viewer; + refinement->visualizationLvl = 3*(visualization_lvl > 2); + refinement->normalize_matchweights = false; + refinement->target_points = 4000; +////double register_setup_start = getTime(); + + reglib::CloudData * cd1 = sweep->getCD(sweep->points.size()); + refinement->setDst(cd1); + + fullmodel->points = frames.front()->getSuperPoints(Eigen::Matrix4d::Identity(),1,false); + reglib::CloudData * cd2 = fullmodel->getCD(fullmodel->points.size()); + refinement->setSrc(cd2); +//////printf("register_setup_start: %5.5f\n",getTime()-register_setup_start); +////double register_compute_start = getTime(); + Eigen::Matrix4d guess = both_unrefined[1]*both_unrefined[0].inverse(); + + reglib::FusionResults fr = refinement->getTransform(guess); + Eigen::Matrix4d offset = fr.guess*guess.inverse(); + for(unsigned int i = 1; i < both_unrefined.size(); i++){ + both_unrefined[i] = offset*both_unrefined[i]; + } + + delete refinement; + + fullmodel->points.clear(); + delete cd1; + delete cd2; + + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.25); bgmassreg->timeout = 300; bgmassreg->viewer = viewer; bgmassreg->use_surface = true; bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = visualization_lvl; + bgmassreg->visualizationLvl = visualization_lvl > 1; bgmassreg->maskstep = 5; bgmassreg->nomaskstep = 5; bgmassreg->nomask = true; @@ -815,6 +840,13 @@ int processMetaroom(std::string path, bool store_old_xml = true){ reglib::Model * fullmodel = getAVMetaroom(path); + printf("fullmodel->frames.size() = %i\n",fullmodel->frames.size()); + +// fullmodel->fullDelete(); +// delete fullmodel; +// return 0; + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( fullmodel, reg); mu->occlusion_penalty = 15; @@ -825,17 +857,16 @@ int processMetaroom(std::string path, bool store_old_xml = true){ std::vector fr; std::vector mm; fullmodel->getData(po, fr, mm); - //fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); - //fullmodel->recomputeModelPoints(); - - - - + // fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); + // fullmodel->recomputeModelPoints(); // SemanticRoom observation = SemanticRoomXMLParser::loadRoomFromXML(path,false); // Eigen::Matrix4f roomTransform = observation.getRoomTransform(); // printf("roomTransform\n"); // std::cout << roomTransform << std::endl; +// return 0; +//exit(0); + DynamicObjectXMLParser objectparser(sweep_folder, true); std::string current_waypointid = current_roomData.roomWaypointId; @@ -845,13 +876,23 @@ int processMetaroom(std::string path, bool store_old_xml = true){ int prevind = -1; std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); for (unsigned int i = 0; i < sweep_xmls.size(); i++){ + printf("prev: %s\n",sweep_xmls[i].c_str()); SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); std::string other_waypointid = other_roomData.roomWaypointId; if(sweep_xmls[i].compare(path) == 0){break;} if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} } - int nextind = prevind +2; + + int nextind = sweep_xmls.size(); + for (unsigned int i = (sweep_xmls.size()-1); i >= 0 ; i--){ + printf("next: %s\n",sweep_xmls[i].c_str()); + SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); + std::string other_waypointid = other_roomData.roomWaypointId; + + if(sweep_xmls[i].compare(path) == 0){break;} + if(other_waypointid.compare(current_waypointid) == 0){nextind = i;} + } std::vector< reglib::Model * > models; models.push_back(fullmodel); @@ -874,10 +915,11 @@ int processMetaroom(std::string path, bool store_old_xml = true){ reglib::Model * nxt = getAVMetaroom(next); bgs.push_back(nxt); } - +printf("bgs.size() = %i\n",bgs.size()); if(bgs.size() > 0){ auto sweep = SimpleXMLParser::loadRoomFromXML(path, std::vector{},false); + printf("models.front()->frames.size() = %i\n",models.front()->frames.size()); quasimodo_brain::segment(bgs,models,internal,external,dynamic,visualization_lvl > 0); remove_old_seg(sweep_folder); @@ -1210,7 +1252,7 @@ void trainMetaroom(std::string path){ bgmassreg->stopval = 0.0005; bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); - exit(0); + savePoses(overall_folder+"/"+posepath,bgmfr.poses,17); fullmodel->fullDelete(); delete fullmodel; diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp index 76136515..bb7a57c2 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp @@ -149,7 +149,7 @@ FusionResults RegistrationRefinement::getTransform(Eigen::MatrixXd guess){ double score = 0; stop = 99999; - //if(visualizationLvl >= 2){show(X,Y);} + if(visualizationLvl >= 3){show(X,Y);} //printf("X: %i %i Y: %i %i\n",X.cols(), X.rows(),Y.cols(), Y.rows()); @@ -284,7 +284,7 @@ bool timestopped = false; W = W.array()*rangeW.array()*rangeW.array(); - if(visualizationLvl >= 3){ + if(visualizationLvl >= 4){ //printf("start show\n"); unsigned int s_nr_data = X.cols(); unsigned int d_nr_data = Y.cols(); From a0455426d1a264aefffca32671bba0fa00a63560 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 27 Oct 2016 09:36:33 +0200 Subject: [PATCH 172/255] ... --- .../quasimodo_brain/src/AnnotationTool.cpp | 40 +- quasimodo/quasimodo_brain/src/Util/Util.cpp | 9 +- quasimodo/quasimodo_brain/src/Util/Util.h | 2 +- .../metaroom_additional_view_processing.cpp | 198 +++++++-- .../quasimodo_models/src/core/RGBDFrame.cpp | 28 +- .../src/modelupdater/ModelUpdater.cpp | 376 +++++++++++++----- 6 files changed, 488 insertions(+), 165 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp index b0b143ff..5476db8a 100644 --- a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp +++ b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp @@ -636,6 +636,11 @@ bool annotate(std::string path){ if(c == '2'){state = 1;} if(c == '3'){state = 2;} if(c == '4'){state = 3;} + if(c == 'j' || c == 'q'){ + classname = "segmentation junk"; + instancename = "segmentation junk"; + break; + } if(c == 10){ state = 0;} } } @@ -649,15 +654,14 @@ bool annotate(std::string path){ std::ifstream file (object, ios::in|ios::binary|ios::ate); if (file.is_open()){ size = file.tellg(); - memblock = new char [size]; + memblock = new char [size+1]; file.seekg (0, ios::beg); file.read (memblock, size); file.close(); + memblock[size] = 0; std::string filedata = std::string(memblock); - //",found1b+1); printf("found: %i\n",found1); + printf("found2: %i\n",found2); + + filedata.replace(found1b+1,found2-found1b," classname=\""+classname+"\""+" instancename=\""+instancename+"\""+" tags=\""+tags+"\">"); + + std::ofstream myfile; + myfile.open (object); + myfile << filedata; + myfile.close(); + std::cout << "the entire file content is in memory" << std::endl; + std::cout << filedata << std::endl << std::endl; +/* std::string frontpart = filedata.substr(0,found1b+1); std::string endpart = filedata.substr(found2,filedata.size()-found2-1); @@ -681,11 +696,13 @@ bool annotate(std::string path){ std::string total = frontpart+" classname=\""+classname+"\""+" instancename=\""+instancename+"\""+" tags=\""+tags+"\""+endpart; + std::string total2 = filedata; + total2.replace(found1b+1,found2-found1b," classname=\""+classname+"\""+" instancename=\""+instancename+"\""+" tags=\""+tags+"\">");//std::string total2 = frontpart+" classname=\""+classname+"\""+" instancename=\""+instancename+"\""+" tags=\""+tags+"\""+endpart; - std::ofstream myfile; - myfile.open (object); - myfile << total; - myfile.close(); +// std::ofstream myfile; +// myfile.open (object); +// myfile << total2; +// myfile.close(); //std::cout << std::endl << std::endl << std::endl << total << std::endl; //exit(0); @@ -694,8 +711,15 @@ bool annotate(std::string path){ // found = path.find("//"); // } + printf("size: %i\n",size); + printf("filedata.size(): %i\n",filedata.size()); + std::cout << "the entire file content is in memory" << std::endl; - std::cout << total << std::endl; + std::cout << filedata << std::endl << std::endl; + std::cout << total << std::endl << std::endl; + std::cout << total2 << std::endl << std::endl; +*/ + delete[] memblock; }else{ diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 5594fac4..33bc3174 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -255,7 +255,10 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ printf("load_metaroom_model(%s)\n",sweep_folder.c_str()); SimpleXMLParser parser; - SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); + std::vector dummy; + dummy.push_back("RoomIntermediateCloud"); + dummy.push_back("IntermediatePosition"); + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml",dummy); reglib::Model * sweepmodel = 0; @@ -295,7 +298,7 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ return sweepmodel; } -void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg){ +void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, int debugg){ double startTime = getTime(); printf("running segment method\n"); boost::shared_ptr viewer; @@ -308,7 +311,7 @@ void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); massregmod->timeout = 1200; massregmod->viewer = viewer; - massregmod->visualizationLvl = 0; + massregmod->visualizationLvl = debugg > 1; massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massregmod->nomask = true; diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index f471776d..4b62c7a3 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -55,7 +55,7 @@ std::vector getRegisteredViewPoses(const std::string& poses_fil Eigen::Matrix4d getMat(tf::StampedTransform tf); reglib::Model * load_metaroom_model(std::string sweep_xml); -void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, bool debugg = false); +void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, int debugg = 0); std::vector loadModelsXML(std::string path); } diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 76ca9767..c47ebed1 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -89,6 +89,11 @@ #include +#include +#include +#include +#include + std::vector< ros::ServiceServer > m_DynamicObjectsServiceServers; std::vector< ros::ServiceServer > m_GetDynamicObjectServiceServers; @@ -117,9 +122,11 @@ using namespace semantic_map_load_utilties; std::string overall_folder = ""; boost::shared_ptr viewer; -int visualization_lvl = 0; -std::string outtopic = ""; -std::string modelouttopic = ""; +int visualization_lvl = 0; +int visualization_lvl_regref = 0; +int visualization_lvl_regini = 0; +std::string outtopic = ""; +std::string modelouttopic = ""; ros::Publisher out_pub; ros::Publisher model_pub; @@ -138,6 +145,11 @@ bool recomputeRelativePoses = false; bool testDynamicObjectServiceCallback(std::string path); bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res); +void signal_callback_handler(int signum){ + printf("Caught signal %d\n",signum); + exit(signum); +} + void remove_old_seg(std::string sweep_folder){ DIR *dir; struct dirent *ent; @@ -366,7 +378,7 @@ int readNumberOfViews(std::string xmlFile){ return count; } -void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses){ +void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses, bool compute_edges = true){ QFile file(xmlFile.c_str()); @@ -516,7 +528,7 @@ void setBaseSweep(std::string path){ } } -reglib::Model * processAV(std::string path){ +reglib::Model * processAV(std::string path, bool compute_edges = true){ printf("processAV: %s\n",path.c_str()); int slash_pos = path.find_last_of("/"); @@ -617,7 +629,7 @@ reglib::Model * processAV(std::string path){ reglib::RegistrationRefinement * refinement = new reglib::RegistrationRefinement(); refinement->viewer = viewer; - refinement->visualizationLvl = 3*(visualization_lvl > 2); + refinement->visualizationLvl = visualization_lvl_regini; refinement->normalize_matchweights = false; refinement->target_points = 4000; ////double register_setup_start = getTime(); @@ -649,7 +661,7 @@ reglib::Model * processAV(std::string path){ bgmassreg->viewer = viewer; bgmassreg->use_surface = true; bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = visualization_lvl > 1; + bgmassreg->visualizationLvl = visualization_lvl_regref; bgmassreg->maskstep = 5; bgmassreg->nomaskstep = 5; bgmassreg->nomask = true; @@ -767,7 +779,7 @@ std::vector readPoseXML(std::string xmlFile){ return poses; } -reglib::Model * getAVMetaroom(std::string path){ +reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true){ printf("processing: %s\n",path.c_str()); if ( ! boost::filesystem::exists( path ) ){return 0;} @@ -789,7 +801,7 @@ reglib::Model * getAVMetaroom(std::string path){ } SimpleXMLParser parser; - SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); + SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path,std::vector{"RoomIntermediateCloud","IntermediatePosition"}); int metaroom_nrviews = current_roomData.vIntermediateRoomClouds.size(); @@ -810,10 +822,10 @@ reglib::Model * getAVMetaroom(std::string path){ fullmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); } - readViewXML(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); + readViewXML(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes,compute_edges); fullmodel->recomputeModelPoints(); }else{ - fullmodel = processAV(path); + fullmodel = processAV(path,compute_edges); writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); } @@ -821,6 +833,8 @@ reglib::Model * getAVMetaroom(std::string path){ return fullmodel; } +int totalcounter = 0; + int processMetaroom(std::string path, bool store_old_xml = true){ quasimodo_brain::cleanPath(path); @@ -835,8 +849,12 @@ int processMetaroom(std::string path, bool store_old_xml = true){ QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); store_old_xml = objectFiles.size() == 0; + printf("current_roomData starting to load\n"); + SimpleXMLParser parser; - SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path); + SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path,std::vector{"RoomIntermediateCloud","IntermediatePosition"}); + + printf("current_roomData loaded\n"); reglib::Model * fullmodel = getAVMetaroom(path); @@ -876,7 +894,6 @@ int processMetaroom(std::string path, bool store_old_xml = true){ int prevind = -1; std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); for (unsigned int i = 0; i < sweep_xmls.size(); i++){ - printf("prev: %s\n",sweep_xmls[i].c_str()); SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); std::string other_waypointid = other_roomData.roomWaypointId; @@ -886,7 +903,6 @@ int processMetaroom(std::string path, bool store_old_xml = true){ int nextind = sweep_xmls.size(); for (unsigned int i = (sweep_xmls.size()-1); i >= 0 ; i--){ - printf("next: %s\n",sweep_xmls[i].c_str()); SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); std::string other_waypointid = other_roomData.roomWaypointId; @@ -909,12 +925,12 @@ int processMetaroom(std::string path, bool store_old_xml = true){ bgs.push_back(bg); } - if(nextind < sweep_xmls.size()){ - std::string next = sweep_xmls[nextind]; - printf("next: %s\n",next.c_str()); - reglib::Model * nxt = getAVMetaroom(next); - bgs.push_back(nxt); - } +// if(nextind < sweep_xmls.size()){ +// std::string next = sweep_xmls[nextind]; +// printf("next: %s\n",next.c_str()); +// reglib::Model * nxt = getAVMetaroom(next); +// bgs.push_back(nxt); +// } printf("bgs.size() = %i\n",bgs.size()); if(bgs.size() > 0){ auto sweep = SimpleXMLParser::loadRoomFromXML(path, std::vector{},false); @@ -1057,6 +1073,7 @@ printf("bgs.size() = %i\n",bgs.size()); // std::cout << objectpcd.substr(0,objectpcd.size()-4) << std::endl; sprintf(buf,"%s/dynamic_obj%10.10i.xml",sweep_folder.c_str(),dynamicCounter-1); + printf("saving dynamic objec: %s\n",buf); QFile file(buf); if (file.exists()){file.remove();} if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<rgb.data); + char buf [1024]; sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),dynamicCounter-1,imgnr[j]); cv::imwrite(buf, masks[j] ); + + int width = masks[j].cols; + int height = masks[j].rows; + + int maxw = 0; + int minw = width; + int maxh = 0; + int minh = height; + for(int w = 0; w < width; w++){ + for(int h = 0; h < height; h++){ + if(masks[j].data[h*width+w] != 0){ + maxw = std::max(maxw,w); + minw = std::min(minw,w); + maxh = std::max(maxh,h); + minh = std::min(minh,h); + } + } + } + + double ratio = 0.15; + int diffw = maxw-minw; + int diffh = maxh-minh; + + printf("w: %i %i ",minw,maxw); + printf("h: %i %i ",minh,maxh); + printf("diff: %i %i\n",diffw,diffh); + maxw = std::min(width-1 ,int(maxw+ratio*diffw)); + minw = std::max(0 ,int(minw-ratio*diffw)); + maxh = std::min(height-1,int(maxh+ratio*diffh)); + minh = std::max(0 ,int(minh-ratio*diffh)); + +// cv::namedWindow( "mask" , cv::WINDOW_AUTOSIZE ); cv::imshow( "mask", masks[j] ); +// cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb ); +// cv::waitKey(0); + + diffw = maxw-minw; + diffh = maxh-minh; + printf("w: %i %i ",minw,maxw); + printf("h: %i %i ",minh,maxh); + printf("diff: %i %i\n",diffw,diffh); + + + // Transform it into the C++ cv::Mat format + cv::Mat image = frame->rgb.clone(); + + // Setup a rectangle to define your region of interest + cv::Rect myROI(minw, minh, diffw, diffh); + + // Crop the full image to that image contained by the rectangle myROI + // Note that this doesn't copy the data + cv::Mat localimg = image(myROI); + + + char buf2 [1024]; + sprintf(buf2,"/home/johane/imgregion/region%10.10i.png",totalcounter++); + cv::imwrite(buf2, localimg ); + + +// cv::Mat localimg; +// localimg.create(diffh,diffw,CV_8UC3); + +// int width2 = maxw-minw; +// for(int w = minw; w < maxw; w++){ +// for(int h = minw; h < maxh; h++){ +// int w2 = w-minw; +// int h2 = h-minh; +// int ind = h*width+w; +// int ind1 = h2*width2+w2; +// localimg.data[3*ind1+0] = rgbdata[3*ind+0]; +// localimg.data[3*ind1+1] = rgbdata[3*ind+1]; +// localimg.data[3*ind1+2] = rgbdata[3*ind+2]; +// } +// } + +// cv::namedWindow( "mask" , cv::WINDOW_AUTOSIZE ); cv::imshow( "mask", masks[j] ); +// cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb ); +// cv::namedWindow( "localimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "localimg", localimg ); +// cv::waitKey(0); + + printf("saving dynamic mask: dynamicmask_%i_%i.png\n",dynamicCounter-1,imgnr[j]); + sprintf(buf,"dynamicmask_%i_%i.png",dynamicCounter-1,imgnr[j]); xmlWriter->writeStartElement("Mask"); xmlWriter->writeAttribute("filename", QString(buf)); @@ -1186,7 +1288,6 @@ printf("bgs.size() = %i\n",bgs.size()); char buf [1024]; sprintf(buf,"%s/movingmask_%i_%i.png",sweep_folder.c_str(),movingCounter-1,imgnr[j]); cv::imwrite(buf, masks[j] ); - sprintf(buf,"movingmask_%i_%i.png",movingCounter-1,imgnr[j]); xmlWriter->writeStartElement("Mask"); xmlWriter->writeAttribute("filename", QString(buf)); @@ -1472,14 +1573,32 @@ bool getDynamicObjectServiceCallback(GetDynamicObjectServiceRequest &req, GetDyn return true; } -bool segmentRaresFiles(std::string path){ +bool segmentRaresFiles(std::string path, bool resegment){ printf("bool segmentRaresFiles(%s)\n",path.c_str()); vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(path); for (auto sweep_xml : sweep_xmls) { printf("sweep_xml: %s\n",sweep_xml.c_str()); - processMetaroom(sweep_xml); - ///reglib::Model * fullmodel = processAV(sweep_xml); + + quasimodo_brain::cleanPath(sweep_xml); + int slash_pos = sweep_xml.find_last_of("/"); + std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; + QStringList segoutput = QDir(sweep_folder.c_str()).entryList(QStringList("segoutput.txt")); + //QStringList movFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*moving_obj*.xml")); + + //printf("sweep_folder: %s\n",sweep_folder.c_str()); + printf("segoutput %i\n",segoutput.size()); + if(resegment || segoutput.size() == 0){ + + std::ofstream myfile; + myfile.open (sweep_folder+"segoutput.txt"); + myfile << "dummy"; + myfile.close(); + + + processMetaroom(sweep_xml); + } + } return false; } @@ -1526,7 +1645,6 @@ void setLargeStack(){ } int main(int argc, char** argv){ - bool baseSetting = true; bool once = false; @@ -1545,22 +1663,19 @@ int main(int argc, char** argv){ std::vector trainMetarooms; std::vector sendMetaroomToServers; std::vector processMetarooms; + + std::vector raresfiles_resegment; std::vector raresfiles; + bool resegment = false; + int inputstate = 0; for(int i = 1; i < argc;i++){ printf("input: %s\n",argv[i]); if( std::string(argv[i]).compare("-intopic") == 0){ inputstate = 0;} else if(std::string(argv[i]).compare("-outtopic") == 0){ inputstate = 1;} else if(std::string(argv[i]).compare("-file") == 0){ inputstate = 2;} - else if(std::string(argv[i]).compare("-v") == 0){ - viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); - viewer->setBackgroundColor (0.5, 0, 0.5); - viewer->addCoordinateSystem (1.0); - viewer->initCameraParameters (); - visualization_lvl = 1; - inputstate = 3; - } + else if(std::string(argv[i]).compare("-v") == 0){ visualization_lvl = 1;inputstate = 3;} else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 4;} else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} @@ -1574,9 +1689,12 @@ int main(int argc, char** argv){ else if(std::string(argv[i]).compare("-statusmsg") == 0){ inputstate = 14;} else if(std::string(argv[i]).compare("-files") == 0){ inputstate = 15;} else if(std::string(argv[i]).compare("-baseSweep") == 0){ inputstate = 16;} + else if(std::string(argv[i]).compare("-resegment") == 0){ resegment = true;} else if(std::string(argv[i]).compare("-once") == 0){ once = true;} else if(std::string(argv[i]).compare("-nobase") == 0){ baseSetting = false;} else if(std::string(argv[i]).compare("-recomputeRelativePoses") == 0){ recomputeRelativePoses = true;} + else if(std::string(argv[i]).compare("-v_init") == 0){ visualization_lvl_regini = 1;inputstate = 17;} + else if(std::string(argv[i]).compare("-v_reg") == 0){ visualization_lvl_regref = 1;inputstate = 18;} else if(inputstate == 0){ segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); }else if(inputstate == 1){ @@ -1609,12 +1727,24 @@ int main(int argc, char** argv){ m_PublisherStatuss.push_back(n.advertise(std::string(argv[i]), 1000)); }else if(inputstate == 15){ raresfiles.push_back(std::string(argv[i])); + raresfiles_resegment.push_back(resegment); }else if(inputstate == 16){ setBaseSweep(std::string(argv[i])); + }else if(inputstate == 17){ + visualization_lvl_regini = atoi(argv[i]); + }else if(inputstate == 18){ + visualization_lvl_regref = atoi(argv[i]); } } + if(visualization_lvl != 0 || visualization_lvl_regref != 0 || visualization_lvl_regini != 0){ + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (0.5, 0, 0.5); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + } + if(baseSetting){ if(m_PublisherStatuss.size() == 0){ m_PublisherStatuss.push_back(n.advertise("/local_metric_map/status", 1000)); @@ -1651,7 +1781,7 @@ int main(int argc, char** argv){ printf("overall_folder: %s\n",overall_folder.c_str()); - for(unsigned int i = 0; i < raresfiles.size(); i++){ segmentRaresFiles( raresfiles[i]);} + for(unsigned int i = 0; i < raresfiles.size(); i++){ segmentRaresFiles( raresfiles[i], raresfiles_resegment[i]);} for(unsigned int i = 0; i < trainMetarooms.size(); i++){ trainMetaroom( trainMetarooms[i]);} for(unsigned int i = 0; i < processMetarooms.size(); i++){ processMetaroom( processMetarooms[i]);} for(unsigned int i = 0; i < sendMetaroomToServers.size(); i++){ sendMetaroomToServer( sendMetaroomToServers[i]);} diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 869a379e..fd15b135 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -784,7 +784,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt DistanceWeightFunction2PPR3 * funcZ = new DistanceWeightFunction2PPR3(); funcZ->zeromean = true; //funcZ->startreg = 0.001; - funcZ->startreg = 0.000001; + funcZ->startreg = 0.0001; funcZ->debugg_print = false; funcZ->bidir = false; funcZ->maxp = 0.999999; @@ -928,7 +928,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt DistanceWeightFunction2PPR2 * funcG = new DistanceWeightFunction2PPR2(); DistanceWeightFunction2PPR2 * funcB = new DistanceWeightFunction2PPR2(); funcR->zeromean = funcG->zeromean = funcB->zeromean = true; - funcR->maxp = funcG->maxp = funcB->maxp = 0.9999; + funcR->maxp = funcG->maxp = funcB->maxp = 0.999999999999999999; funcR->startreg = funcG->startreg = funcB->startreg = 1.0; funcR->debugg_print = funcG->debugg_print = funcB->debugg_print = false; funcR->maxd = funcG->maxd = funcB->maxd = 256.0; @@ -936,7 +936,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt funcR->fixed_histogram_size = funcG->fixed_histogram_size = funcB->fixed_histogram_size = true; funcR->startmaxd = funcG->startmaxd = funcB->startmaxd = funcR->maxd; funcR->starthistogram_size = funcG->starthistogram_size = funcB->starthistogram_size = funcR->histogram_size; - funcR->blurval = funcG->blurval = funcB->blurval = 0.005; + //funcR->blurval = funcG->blurval = funcB->blurval = 0.005; + funcR->blurval = funcG->blurval = funcB->blurval = 0.5; funcR->stdval2 = stdvalR; funcR->maxnoise = stdvalR; @@ -1000,7 +1001,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float * cenmsdata = (float*)cenms.data; for(int i = 0; i < nr_pixels; i++){ double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]); - edgep = std::max(0.0001,edgep); + edgep = std::max(0.000000000000000000001,edgep); cenmsdata[3*i+0] = edgep; cenmsdata[3*i+1] = edgep; cenmsdata[3*i+2] = edgep; @@ -1482,18 +1483,25 @@ std::vector< std::vector > RGBDFrame::getImageProbs(bool depthonly){ dyc.resize(nr_pixels); for(unsigned int i = 0; i < nr_pixels;i++){ - dxc[i] = 1.0-dedata[3*i+1]; - dyc[i] = 1.0-dedata[3*i+2]; + //dxc[i] = 1.0-std::max(dedata[3*i+1],cedata[3*i+1]); + //dyc[i] = 1.0-std::max(dedata[3*i+2],cedata[3*i+2]); + + + if(!depthonly){ if(!det_dilatedata[i]){ - dxc[i] = 1.0-std::max(dedata[3*i+1],cedata[3*i+1]); - dyc[i] = 1.0-std::max(dedata[3*i+2],cedata[3*i+2]); + dxc[i] = 1.0-std::max(dedata[3*i+1],1.0f*cedata[3*i+1]); + dyc[i] = 1.0-std::max(dedata[3*i+2],1.0f*cedata[3*i+2]); }else{ - dxc[i] = 1.0-std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); - dyc[i] = 1.0-std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); + dxc[i] = 1.0-std::max(dedata[3*i+1],1.0f*cedata[3*i+1]); + dyc[i] = 1.0-std::max(dedata[3*i+2],1.0f*cedata[3*i+2]); } + }else{ + dxc[i] = 1.0-dedata[3*i+1]; + dyc[i] = 1.0-dedata[3*i+2]; } + } // cv::namedWindow( "Colour edges" , cv::WINDOW_AUTOSIZE ); cv::imshow( "Colour edges", ce); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 89ff3489..b0cab8f0 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2121,6 +2121,7 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & double p_overlap_angle = nfunc->getProb(1-surface_angle); double p_overlap = dfunc->getProb(d); double p_occlusion = dfunc->getProbInfront(d);//std::max(1 - 1e-6,std::max(1e-6,dfunc->getProbInfront(d))); + double p_behind = 1-p_overlap-p_occlusion; p_overlap *= p_overlap_angle; @@ -2142,12 +2143,12 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & double nolp = 1-olp; nolp *= (1-p_overlap); //overlaps[src_ind] = std::min(0.9999,std::max(olp,p_overlap)); - overlaps[src_ind] = std::min(0.9999,1-nolp); - // - //overlaps[src_ind] = std::min(0.9999,std::max(overlaps[src_ind],p_overlap)); - //double prev = 1.0-occlusions[src_ind]; + overlaps[src_ind] = std::min(0.999999999,1-nolp); occlusions[src_ind] += p_occlusion;//std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); notocclusions[src_ind]++; + + //overlaps[src_ind] = std::min(0.9999,std::max(overlaps[src_ind],p_overlap)); + //double prev = 1.0-occlusions[src_ind]; //notocclusions[src_ind] *= 1.0-p_occlusion;//std::min(0.9,std::max(occlusions[src_ind],p_occlusion)); @@ -2197,6 +2198,95 @@ void ModelUpdater::getDynamicWeights(bool store_distance, std::vector & } } +int setupPriors(int method,float current_occlusions, float current_overlaps, float bg_occlusions, float bg_overlaps, bool valid, + float & prior_moving, float & prior_dynamic, float & prior_static, float & foreground_weight, float & background_weight){ + float bias = 0.1; + float maxdiff = 0.001; + + foreground_weight = 0; + background_weight = 0; + + if(method == 0){ + if(valid){ + float p_moving_or_dynamic = std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f); + float p_moving = 0.5f*p_moving_or_dynamic + current_occlusions; + float p_dynamic = 0.5f*p_moving_or_dynamic + std::max((bg_occlusions-current_occlusions) * current_overlaps,0.0f); + float p_static = std::max(bg_overlaps-p_moving-p_dynamic,0.0f); + + float leftover = 1.0f-p_moving-p_dynamic-p_static; + float notMoving = current_overlaps; + + float p_moving_leftover = 0.5*leftover*(1-notMoving); //(1-notMoving)*leftover/4.0; + float p_dynamic_leftover = 0.5*leftover*notMoving + 0.5*leftover*(1-notMoving); //(1-notMoving)*leftover/4.0; + float p_static_leftover = 0.5*leftover*notMoving; + + prior_moving = p_moving+p_moving_leftover; + prior_dynamic = p_dynamic+p_dynamic_leftover; + prior_static = p_static+p_static_leftover; + + double p_fg = std::min( 1.0f-maxdiff ,std::max( maxdiff , prior_moving+prior_dynamic))-bias; + double p_bg = std::min( 1.0f-maxdiff ,std::max( maxdiff , prior_static))+bias; + double norm = p_fg + p_bg; + p_fg /= norm; + p_bg /= norm; + + + if(norm > 0){ + foreground_weight = -log(p_fg); + background_weight = -log(p_bg); + } + }else{ + prior_moving = 0; + prior_dynamic = 0.5; + prior_static = 0.5; + } + } + + if(method == 1){ + if(valid){ + + //Prob behind all bg + float prob_behind = 1-(bg_overlaps+bg_occlusions); + float leftover = 1-prob_behind; + + //float p_moving_or_dynamic = std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f); + float p_moving = std::min(current_occlusions,leftover);//prob_behind;//current_occlusions;//0.5f*p_moving_or_dynamic + current_occlusions; + leftover = 1.0f-prob_behind-p_moving; + float p_dynamic = std::min(bg_occlusions,leftover);//std::max(bg_occlusions,leftover);//std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f);//std::max((bg_occlusions-current_occlusions),0.0f);//0.5f*p_moving_or_dynamic + std::max((bg_occlusions-current_occlusions) * current_overlaps,0.0f); + leftover = 1.0f-prob_behind-p_moving-p_dynamic; + float p_static = std::min(bg_overlaps,leftover);//std::max(bg_overlaps-p_moving-p_dynamic,0.0f); + + leftover = 1.0f-p_moving-p_dynamic-p_static; + float notMoving = current_overlaps; + + float p_moving_leftover = 0;//0.25*leftover;//*(1-notMoving); //(1-notMoving)*leftover/4.0; + float p_dynamic_leftover = 0;//0.25*leftover;//0.5*leftover*notMoving + 0.5*leftover*(1-notMoving); //(1-notMoving)*leftover/4.0; + float p_static_leftover = 0;//0.5*leftover;//*notMoving; + + prior_moving = p_moving+p_moving_leftover; + prior_dynamic = p_dynamic+p_dynamic_leftover; + prior_static = p_static+p_static_leftover; + + double p_fg = std::min( 1.0f-maxdiff ,std::max( maxdiff , prior_moving+prior_dynamic-0.01f)); + double p_bg = std::min( 1.0f-maxdiff ,std::max( maxdiff , prior_static+0.01f)); + double norm = p_fg + p_bg; + p_fg /= norm; + p_bg /= norm; + + + if(norm > 0){ + foreground_weight = -log(p_fg); + background_weight = -log(p_bg); + } + }else{ + prior_moving = 0; + prior_dynamic = 0.5; + prior_static = 0.5; + } + } + return 0; +} + void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ double computeMovingDynamicStatic_startTime = getTime(); @@ -2221,8 +2311,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s interframe_connectionStrength.resize(tot_nr_pixels); - int current_point = 0; - float * priors = new float[3*tot_nr_pixels]; + int current_point = 0; + float * priors = new float[3*tot_nr_pixels]; + float * prior_weights = new float[2*tot_nr_pixels]; bool * valids = new bool[tot_nr_pixels]; std::vector dvec; @@ -2322,13 +2413,17 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s double total_dealloctime = 0; double total_Dynw = 0; + float bias = 0.0001; + float maxdiff = bias+0.0001; + double maxprob_same = 0.999999999999999999; + for(unsigned int i = 0; i < frames.size(); i++){ - printf("currently workin on frame %i\n",i); + if(debugg != 0){printf("currently workin on frame %i\n",i);} int offset = offsets[i]; RGBDFrame * frame = frames[i]; + float * normalsdata = (float *)(frame->normals.data); startTime = getTime(); - std::vector< std::vector > probs = frame->getImageProbs();//getImageProbs(frame,9); - //std::vector< std::vector > probs2 = getImageProbs(frame,9); + std::vector< std::vector > probs = frame->getImageProbs(); total_connectiontime += getTime()-startTime; pixel_weights.push_back(probs); @@ -2354,8 +2449,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s current_notocclusions[j] = 0.0; } - double * bg_overlaps = new double[nr_pixels]; - double * bg_occlusions = new double[nr_pixels]; + double * bg_overlaps = new double[nr_pixels]; + double * bg_occlusions = new double[nr_pixels]; double * bg_notocclusions = new double[nr_pixels]; for(unsigned int j = 0; j < nr_pixels; j++){ bg_overlaps[j] = 0; @@ -2365,14 +2460,12 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s total_alloctime += getTime()-startTime; startTime = getTime(); - printf("current\n"); for(unsigned int j = 0; j < frames.size(); j++){ if(i == j){continue;} Eigen::Matrix4d p = poses[i].inverse() * poses[j]; getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], current_overlaps, current_occlusions, current_notocclusions, frames[j],framesp[j],offsets[i],offsets[j],interframe_connectionId,interframe_connectionStrength,false); } - printf("background\n"); for(unsigned int j = 0; j < bgcf.size(); j++){ Eigen::Matrix4d p = poses[i].inverse() * bgcp[j]; getDynamicWeights(false,dvec,nvec,dfunc,nfunc,p.inverse(),frames[i],framesp_test[i],framesp[i], bg_overlaps, bg_occlusions, bg_notocclusions, bgcf[j],bgsp[j],-1,-1,interframe_connectionId,interframe_connectionStrength,false); @@ -2381,14 +2474,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for(unsigned int j = 0; j < nr_pixels; j++){ bg_occlusions[j] = std::min(0.999,bg_occlusions[j]/std::max(1.0,bg_notocclusions[j])); current_occlusions[j] = std::min(0.999,current_occlusions[j]/std::max(1.0,current_notocclusions[j])); - //bg_occlusions[j] = bg_occlusions[j] /bg_notocclusions[j]; - //current_occlusions[j] = current_occlusions[j] /current_notocclusions[j]; } - // for(unsigned int j = 0; j < nr_pixels; j++){ - // bg_occlusions[j] = bg_occlusions[j] /(bg_occlusions[j] +bg_notocclusions[j]); - // current_occlusions[j] = current_occlusions[j] /(current_occlusions[j] +current_notocclusions[j]); - // } total_Dynw += getTime()-startTime; cv::Mat priorsimg; @@ -2398,60 +2485,64 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s startTime = getTime(); unsigned char * detdata = (unsigned char*)(frame->det_dilate.data); - float bias = 0.01; + float bias = 0.0001; for(unsigned int h = 0; h < height;h++){ for(unsigned int w = 0; w < width;w++){ int ind = h*width+w; - valids[offset+ind] = detdata[ind] == 0; + valids[offset+ind] = detdata[ind] == 0 && normalsdata[3*ind] != 2; + +/* if(valids[offset+ind]){ - float p_moving = current_occlusions[ind]; - float p_dynamic = std::max((bg_occlusions[ind]-p_moving)*current_overlaps[ind],0.0); - p_dynamic += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); - p_moving += 0.5*std::max((bg_occlusions[ind]-p_moving)*(1-current_overlaps[ind]),0.0); - float p_static = std::max(0.5*bg_overlaps[ind]-p_moving-p_dynamic,0.0); +// float p_moving = current_occlusions[ind]; +// float p_dynamic = std::max((bg_occlusions[ind]-current_occlusions[ind]) * current_overlaps[ind],0.0); +// p_dynamic += 0.5*std::max((bg_occlusions[ind]-current_occlusions[ind])*(1-current_overlaps[ind]),0.0); +// p_moving += 0.5*std::max((bg_occlusions[ind]-current_occlusions[ind])*(1-current_overlaps[ind]),0.0); + + float p_moving_or_dynamic = std::max((bg_occlusions[ind]-current_occlusions[ind])*(1-current_overlaps[ind]),0.0); + float p_moving = 0.5*p_moving_or_dynamic + current_occlusions[ind]; + float p_dynamic = 0.5*p_moving_or_dynamic + std::max((bg_occlusions[ind]-current_occlusions[ind]) * current_overlaps[ind],0.0); + float p_static = std::max(bg_overlaps[ind]-p_moving-p_dynamic,0.0); + float leftover = 1-p_moving-p_dynamic-p_static; + float notMoving = current_overlaps[ind]; - float p_dyn = p_dynamic + (0.5-bias)*leftover; - float p_other = p_static+p_moving + (0.5+bias)*leftover; + float p_moving_leftover = 0.5*leftover*(1-notMoving); //(1-notMoving)*leftover/4.0; + float p_dynamic_leftover = 0.5*leftover*notMoving + 0.5*leftover*(1-notMoving); //(1-notMoving)*leftover/4.0; + float p_static_leftover = 0.5*leftover*notMoving; - priorsdata[3*ind+2] = p_dyn;//p_moving; - priorsdata[3*ind+1] = p_dyn; - priorsdata[3*ind+0] = p_dyn; + priors[3*(offset+ind)+0] = p_moving+p_moving_leftover; + priors[3*(offset+ind)+1] = p_dynamic+p_dynamic_leftover; + priors[3*(offset+ind)+2] = p_static+p_static_leftover; - priors[3*(offset+ind)+0] = 0; - priors[3*(offset+ind)+1] = p_dyn; - priors[3*(offset+ind)+2] = p_other; - }else{ - priorsdata[3*ind+2] = 0;//p_moving; - priorsdata[3*ind+1] = 0.5; - priorsdata[3*ind+0] = 0.5; + + }else{ + if(detdata[ind] == 0){ + priorsdata[3*ind+2] = 1.0; + priorsdata[3*ind+1] = 0.0; + priorsdata[3*ind+0] = 1.0; + }else{ + priorsdata[3*ind+2] = 1.0; + priorsdata[3*ind+1] = 1.0; + priorsdata[3*ind+0] = 0.0; + } priors[3*(offset+ind)+0] = 0; priors[3*(offset+ind)+1] = 0.5; priors[3*(offset+ind)+2] = 0.5; } - - /* - float notMoving = current_overlaps[ind]; - - float bias = 0.0001; - - float p_moving_leftover = -bias+(1-notMoving)*leftover/4.0; - float p_dynamic_leftover = -bias+0.5*notMoving*leftover + (1-notMoving)*leftover/4.0; - float p_static_leftover = 2.0*bias+0.5*notMoving*leftover + (1-notMoving)*leftover/2.0; - - float p_moving_tot = (1.0-4.0*minprob)*(p_moving+p_moving_leftover) + minprob; - float p_dynamic_tot = (1.0-4.0*minprob)*(p_dynamic+p_dynamic_leftover) + minprob; - float p_static_tot = (1.0-4.0*minprob)*(p_static+p_static_leftover) + 2.0*minprob; - - priors[3*(offset+ind)+0] = p_moving_tot; - priors[3*(offset+ind)+1] = p_dynamic_tot; - priors[3*(offset+ind)+2] = p_static_tot; */ + setupPriors(1, + current_occlusions[ind],current_overlaps[ind],bg_occlusions[ind],bg_overlaps[ind],valids[offset+ind], + priors[3*(offset+ind)+0], priors[3*(offset+ind)+1],priors[3*(offset+ind)+2], + prior_weights[2*(offset+ind)+0], prior_weights[2*(offset+ind)+1]); + + priorsdata[3*ind+0] = priors[3*(offset+ind)+0]; + priorsdata[3*ind+1] = priors[3*(offset+ind)+1]; + priorsdata[3*ind+2] = priors[3*(offset+ind)+2]; if(probs[0][ind] > 0.00000001){frameConnections++;} if(probs[1][ind] > 0.00000001){frameConnections++;} @@ -2460,28 +2551,35 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } - printf("run inference\n"); double start_inf = getTime(); gc::Graph * g = new gc::Graph(nr_pixels,2*nr_pixels); for(unsigned long ind = 0; ind < nr_pixels;ind++){ g -> add_node(); - double p_fg = priors[3*(offset+ind)+1]; - double p_bg = priors[3*(offset+ind)+2]; - double norm = p_fg + p_bg; - if(norm <= 0){ - g -> add_tweights( ind, 0, 0 ); - continue; - } - p_fg /= norm; - p_bg /= norm; +// double p_fg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*(offset+ind)+1]))-bias; +// double p_bg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*(offset+ind)+2] + priors[3*(offset+ind)+0]))+bias; +// double norm = p_fg + p_bg; +//// p_fg /= norm; +//// p_bg /= norm; +// p_fg /= 2*norm; +// p_bg /= 2*norm; +// p_fg += 0.25; +// p_bg += 0.25; + +// if(norm <= 0){ +// g -> add_tweights( ind, 0, 0 ); +// continue; +// } - double weightFG = -log(p_fg); - double weightBG = -log(p_bg); - g -> add_tweights( ind, weightFG, weightBG ); - } +// double weightFG = -log(p_fg); +// double weightBG = -log(p_bg); + + double weightFG = prior_weights[2*(offset+ind)+0]; + double weightBG = prior_weights[2*(offset+ind)+1]; - double maxprob_same = 0.99999999999; + + g -> add_tweights( ind, weightFG, weightBG ); + } for(unsigned int w = 0; w < width;w++){ for(unsigned int h = 0; h < height;h++){ @@ -2507,7 +2605,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s g -> maxflow(); for(unsigned long ind = 0; ind < nr_pixels;ind++){labels[offset+ind] = g->what_segment(ind);} - printf("local inference time: %10.10fs\n\n",getTime()-start_inf); + if(debugg != 0){printf("local inference time: %10.10fs\n\n",getTime()-start_inf);} // cv::Mat edgeimgX; @@ -2529,22 +2627,46 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s // } -// cv::Mat labelimg; -// labelimg.create(height,width,CV_32FC3); -// float * labelimgdata = (float*)labelimg.data; -// for(unsigned long ind = 0; ind < nr_pixels;ind++){ -// double label = g->what_segment(ind); -// labelimgdata[3*ind+0] = 0; -// labelimgdata[3*ind+1] = label; -// labelimgdata[3*ind+2] = 1-label; -// } -// cv::namedWindow( "edgeimgX" , cv::WINDOW_AUTOSIZE ); cv::imshow( "edgeimgX", edgeimgX ); -// cv::namedWindow( "edgeimgY" , cv::WINDOW_AUTOSIZE ); cv::imshow( "edgeimgY", edgeimgY ); -// cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb ); -// cv::namedWindow( "depth" , cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", frame->depth ); -// cv::namedWindow( "priors" , cv::WINDOW_AUTOSIZE ); cv::imshow( "priors", priorsimg ); -// cv::namedWindow( "labelimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "labelimg", labelimg ); -// cv::waitKey(0); + cv::Mat edgeimg; + edgeimg.create(height,width,CV_32FC3); + float * edgedata = (float*)edgeimg.data; + + for(int j = 0; j < width*height; j++){ + edgedata[3*j+0] = 0; + edgedata[3*j+1] = 1-probs[0][j]; + edgedata[3*j+2] = 1-probs[1][j]; + } + + cv::Mat labelimg; + labelimg.create(height,width,CV_32FC3); + float * labelimgdata = (float*)labelimg.data; + for(unsigned long ind = 0; ind < nr_pixels;ind++){ + double label = g->what_segment(ind); + labelimgdata[3*ind+0] = 0; + labelimgdata[3*ind+1] = label; + labelimgdata[3*ind+2] = 1-label; + } + + cv::Mat detrgb; + detrgb.create(height,width,CV_8UC3); + + for(unsigned long ind = 0; ind < nr_pixels;ind++){ + int dd = detdata[ind] == 0; + detrgb.data[3*ind+0] = dd * frame->rgb.data[3*ind+0]; + detrgb.data[3*ind+1] = dd * frame->rgb.data[3*ind+1]; + detrgb.data[3*ind+2] = dd * frame->rgb.data[3*ind+2]; + } + + cv::namedWindow( "edgeimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "edgeimg", edgeimg ); + cv::namedWindow( "det_dilate" , cv::WINDOW_AUTOSIZE ); cv::imshow( "det_dilate", frame->det_dilate); + cv::namedWindow( "det_dilate2" , cv::WINDOW_AUTOSIZE ); cv::imshow( "det_dilate2", detrgb); + cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb ); + cv::namedWindow( "depth" , cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", frame->depth ); + cv::namedWindow( "priors" , cv::WINDOW_AUTOSIZE ); cv::imshow( "priors", priorsimg ); + cv::namedWindow( "labelimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "labelimg", labelimg ); + cv::waitKey(0); + + delete g; @@ -2596,23 +2718,28 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s gc::Graph * g = new gc::Graph(current_point,frameConnections+interframeConnections); for(unsigned long i = 0; i < current_point;i++){ g -> add_node(); - double p_fg = priors[3*i+1]; - double p_bg = priors[3*i+2]; + + double p_fg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*i+1]))-bias; + double p_bg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*i+2] + priors[3*i+0]))+bias; double norm = p_fg + p_bg; + p_fg /= 2*norm; + p_bg /= 2*norm; + p_fg += 0.25; + p_bg += 0.25; + if(norm <= 0){ g -> add_tweights( i, 0, 0 ); continue; } - p_fg /= norm; - p_bg /= norm; + double weightFG = -log(p_fg); double weightBG = -log(p_bg); g -> add_tweights( i, weightFG, weightBG ); } - double maxprob_same = 0.99999999999; + //double maxprob_same = 0.99999999999; for(unsigned int i = 0; i < frames.size(); i++){ int offset = offsets[i]; Camera * camera = frames[i]->camera; @@ -2641,8 +2768,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } - printf("run inference\n"); - std::vector > interframe_connectionId_added; interframe_connectionId_added.resize(interframe_connectionId.size()); @@ -2654,8 +2779,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ double weight = interframe_connectionStrength[i][j]; unsigned long other = interframe_connectionId[i][j]; - //if(weight > 0.01 && fabs(priors[3*i+1] - priors[3*i+2]) > 0.5){//labels[i] != labels[other]){//g->what_segment(i) != g->what_segment(other)){ - if(weight > 0.01 && labels[i] != labels[other]){//g->what_segment(i) != g->what_segment(other)){ + if(weight > 0.01 && labels[i] != labels[other]){ g -> add_edge( i, other, weight, weight ); interframe_connectionId_added[i].push_back(other); @@ -2673,8 +2797,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } - printf("initAdded %f of %i\n",initAdded,interframeConnections); - g -> maxflow(); for(unsigned long ind = 0; ind < current_point;ind++){labels[ind] = g->what_segment(ind);} @@ -2694,7 +2816,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s double adds = 100000; double prob = std::min(adds / diffs,1.0); - printf("diffs: %f adds: %f prob: %f\n",diffs,adds,prob); + printf("diffs: %f adds: %f prob: %f ",diffs,adds,prob); printf("ratio of total diffs: %f\n",diffs/double(frameConnections+interframeConnections)); if(diffs == 0){break;} @@ -2722,13 +2844,12 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } } - printf("trueadds: %f\n",trueadds); g -> maxflow(); for(unsigned long ind = 0; ind < current_point;ind++){labels[ind] = g->what_segment(ind);} tot_inf += getTime()-start_inf1; - printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf); + if(debugg != 0){printf("static inference1 time: %10.10fs total: %10.10f\n\n",getTime()-start_inf1,tot_inf);} if(tot_inf > 90){break;} } @@ -2825,28 +2946,30 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } float * dedata = (float*)(frames[frameind]->de.data); + unsigned short * depthdata = (unsigned short *)(frames[frameind]->depth.data); + if(depthdata[iind] == 0){printf("big giant WTF... file %i line %i\n",__FILE__,__LINE__);continue;} int dir; dir = -1; - if( w > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + if( w > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && depthdata[iind+dir] != 0 && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ objectlabel[cind+dir] = number_of_dynamics; todo.push_back(cind+dir); } dir = 1; - if( w < (width-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + if( w < (width-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && depthdata[iind+dir] != 0 && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ objectlabel[cind+dir] = number_of_dynamics; todo.push_back(cind+dir); } dir = -width; - if( h > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + if( h > 0 && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && depthdata[iind+dir] != 0 && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ objectlabel[cind+dir] = number_of_dynamics; todo.push_back(cind+dir); } dir = width; - if( h < (height-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ + if( h < (height-1) && objectlabel[cind+dir] == 0 && labels[cind+dir] == current_label && depthdata[iind+dir] != 0 && (dedata[3*(iind+dir)+1]+dedata[3*(iind+dir)+2]) < probthresh){ objectlabel[cind+dir] = number_of_dynamics; todo.push_back(cind+dir); } @@ -2860,13 +2983,13 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } - printf("score: %f\n",score); + if(debugg != 0){printf("score: %f\n",score);} labelID.push_back(0); if(score < 200){continue;} - if(debugg && todo.size() > 10000){ + if(false && debugg && todo.size() > 10000){ for(unsigned int i = 0; i < current_point; i++){ cloud->points[i].r = priors[3*i+0]*255.0; cloud->points[i].g = priors[3*i+1]*255.0; @@ -2901,21 +3024,25 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s if(current_label == 1){ labelID.back() = ++nr_obj_dyn; - printf("Dynamic: %f -> %f\n",score,totsum); + if(debugg != 0){printf("Dynamic: %f -> %f\n",score,totsum);} sr.component_dynamic.push_back(todo); sr.scores_dynamic.push_back(score); sr.total_dynamic.push_back(totsum); } if(current_label == 2){ labelID.back() = --nr_obj_mov; - printf("Moving: %f -> %f\n",score,totsum); + if(debugg != 0){printf("Moving: %f -> %f\n",score,totsum);} sr.component_moving.push_back(todo); sr.scores_moving.push_back(score); sr.total_moving.push_back(totsum); } } } + + for(unsigned int i = 0; i < interframe_connectionId.size();i++){interframe_connectionId[i].clear();} interframe_connectionId.clear(); + + for(unsigned int i = 0; i < interframe_connectionStrength.size();i++){interframe_connectionStrength[i].clear();} interframe_connectionStrength.clear(); printf("connectedComponent: %5.5fs\n",getTime()-start_inf); @@ -3013,6 +3140,26 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); viewer->spin(); + cloud_sample->points.clear(); + for(unsigned int i = 0; i < current_point; i++){ + if(rand() % 4 == 0){ + + double p_fg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*i+1]))-bias; + double p_bg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*i+2] + priors[3*i+0]))+bias; + double norm = p_fg + p_bg; + p_fg /= norm; + p_bg /= norm; + + cloud_sample->points.push_back(cloud->points[i]); + cloud_sample->points.back().r = p_fg*255.0;//(priors[3*i+0]+priors[3*i+2])*255.0; + cloud_sample->points.back().g = p_fg*255.0; + cloud_sample->points.back().b = p_fg*255.0;//; + } + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); + cloud_sample->points.clear(); for(unsigned int i = 0; i < current_point; i++){ @@ -3050,10 +3197,21 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s viewer->removeAllPointClouds(); viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); viewer->spin(); + + cloud_sample->points.clear(); + for(unsigned int i = 0; i < current_point; i++){ + if(rand() % 1 == 0){ + cloud_sample->points.push_back(cloud->points[i]); + } + } + viewer->removeAllPointClouds(); + viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); + viewer->spin(); } delete[] valids; delete[] priors; + delete[] prior_weights; printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime); } From 8884d76f812a61a1597ebf8316f11915ada03895 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 28 Oct 2016 22:39:49 +0200 Subject: [PATCH 173/255] ... --- .../metaroom_additional_view_processing.cpp | 46 +- .../DistanceWeightFunction2PPR3.h | 1 + .../include/weightfunctions/Distribution.h | 1 + .../quasimodo_models/src/core/RGBDFrame.cpp | 8 +- .../src/modelupdater/ModelUpdater.cpp | 543 ++++++++---------- .../DistanceWeightFunction2PPR3.cpp | 67 ++- .../src/weightfunctions/Distribution.cpp | 6 +- .../weightfunctions/GaussianDistribution.cpp | 1 + .../GeneralizedGaussianDistribution.cpp | 3 +- 9 files changed, 294 insertions(+), 382 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index c47ebed1..472c693a 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1094,15 +1094,10 @@ printf("bgs.size() = %i\n",bgs.size()); for(unsigned int j = 0; j < masks.size(); j++){ - - reglib::RGBDFrame * frame = mod_fr[imgnr[j]]; - unsigned char * rgbdata = (unsigned char *)(frame->rgb.data); - char buf [1024]; sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),dynamicCounter-1,imgnr[j]); cv::imwrite(buf, masks[j] ); - int width = masks[j].cols; int height = masks[j].rows; @@ -1125,33 +1120,16 @@ printf("bgs.size() = %i\n",bgs.size()); int diffw = maxw-minw; int diffh = maxh-minh; - printf("w: %i %i ",minw,maxw); - printf("h: %i %i ",minh,maxh); - printf("diff: %i %i\n",diffw,diffh); maxw = std::min(width-1 ,int(maxw+ratio*diffw)); minw = std::max(0 ,int(minw-ratio*diffw)); maxh = std::min(height-1,int(maxh+ratio*diffh)); minh = std::max(0 ,int(minh-ratio*diffh)); -// cv::namedWindow( "mask" , cv::WINDOW_AUTOSIZE ); cv::imshow( "mask", masks[j] ); -// cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb ); -// cv::waitKey(0); - diffw = maxw-minw; diffh = maxh-minh; - printf("w: %i %i ",minw,maxw); - printf("h: %i %i ",minh,maxh); - printf("diff: %i %i\n",diffw,diffh); - - // Transform it into the C++ cv::Mat format - cv::Mat image = frame->rgb.clone(); - - // Setup a rectangle to define your region of interest + cv::Mat image = mod_fr[imgnr[j]]->rgb.clone(); cv::Rect myROI(minw, minh, diffw, diffh); - - // Crop the full image to that image contained by the rectangle myROI - // Note that this doesn't copy the data cv::Mat localimg = image(myROI); @@ -1159,28 +1137,6 @@ printf("bgs.size() = %i\n",bgs.size()); sprintf(buf2,"/home/johane/imgregion/region%10.10i.png",totalcounter++); cv::imwrite(buf2, localimg ); - -// cv::Mat localimg; -// localimg.create(diffh,diffw,CV_8UC3); - -// int width2 = maxw-minw; -// for(int w = minw; w < maxw; w++){ -// for(int h = minw; h < maxh; h++){ -// int w2 = w-minw; -// int h2 = h-minh; -// int ind = h*width+w; -// int ind1 = h2*width2+w2; -// localimg.data[3*ind1+0] = rgbdata[3*ind+0]; -// localimg.data[3*ind1+1] = rgbdata[3*ind+1]; -// localimg.data[3*ind1+2] = rgbdata[3*ind+2]; -// } -// } - -// cv::namedWindow( "mask" , cv::WINDOW_AUTOSIZE ); cv::imshow( "mask", masks[j] ); -// cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb ); -// cv::namedWindow( "localimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "localimg", localimg ); -// cv::waitKey(0); - printf("saving dynamic mask: dynamicmask_%i_%i.png\n",dynamicCounter-1,imgnr[j]); sprintf(buf,"dynamicmask_%i_%i.png",dynamicCounter-1,imgnr[j]); diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR3.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR3.h index 38a22e01..609862f5 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR3.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2PPR3.h @@ -118,6 +118,7 @@ class DistanceWeightFunction2PPR3 : public DistanceWeightFunction2 virtual void reset(); virtual std::string getString(); virtual double getInd(double d, bool debugg = false); + virtual double getDfromInd(double ind, bool debugg = false); }; } diff --git a/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h b/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h index 5f17e13e..f21a10f7 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/Distribution.h @@ -15,6 +15,7 @@ class Distribution public: double regularization; double mean; + bool debugg_print; Distribution(); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index fd15b135..e24e0b1b 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -1491,11 +1491,11 @@ std::vector< std::vector > RGBDFrame::getImageProbs(bool depthonly){ if(!depthonly){ if(!det_dilatedata[i]){ - dxc[i] = 1.0-std::max(dedata[3*i+1],1.0f*cedata[3*i+1]); - dyc[i] = 1.0-std::max(dedata[3*i+2],1.0f*cedata[3*i+2]); + dxc[i] = 1.0-std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); + dyc[i] = 1.0-std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); }else{ - dxc[i] = 1.0-std::max(dedata[3*i+1],1.0f*cedata[3*i+1]); - dyc[i] = 1.0-std::max(dedata[3*i+2],1.0f*cedata[3*i+2]); + dxc[i] = 1.0-std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); + dyc[i] = 1.0-std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); } }else{ dxc[i] = 1.0-dedata[3*i+1]; diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index b0cab8f0..d86ba52a 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2205,6 +2205,9 @@ int setupPriors(int method,float current_occlusions, float current_overlaps, flo foreground_weight = 0; background_weight = 0; + prior_moving = 0.0; + prior_dynamic = 0.0; + prior_static = 0.0; if(method == 0){ if(valid){ @@ -2235,53 +2238,135 @@ int setupPriors(int method,float current_occlusions, float current_overlaps, flo foreground_weight = -log(p_fg); background_weight = -log(p_bg); } - }else{ - prior_moving = 0; - prior_dynamic = 0.5; - prior_static = 0.5; } } if(method == 1){ if(valid){ + float minprob = 0.01; + float bias = 0.001; + float overlap_same_prob = 0.95; + //Prob behind all bg float prob_behind = 1-(bg_overlaps+bg_occlusions); float leftover = 1-prob_behind; - //float p_moving_or_dynamic = std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f); - float p_moving = std::min(current_occlusions,leftover);//prob_behind;//current_occlusions;//0.5f*p_moving_or_dynamic + current_occlusions; - leftover = 1.0f-prob_behind-p_moving; - float p_dynamic = std::min(bg_occlusions,leftover);//std::max(bg_occlusions,leftover);//std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f);//std::max((bg_occlusions-current_occlusions),0.0f);//0.5f*p_moving_or_dynamic + std::max((bg_occlusions-current_occlusions) * current_overlaps,0.0f); - leftover = 1.0f-prob_behind-p_moving-p_dynamic; - float p_static = std::min(bg_overlaps,leftover);//std::max(bg_overlaps-p_moving-p_dynamic,0.0f); + float p_moving_or_dynamic = std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f); + float p_moving = 0.5f*p_moving_or_dynamic + current_occlusions; + float p_dynamic = 0.5f*p_moving_or_dynamic + std::max((bg_occlusions-current_occlusions) * current_overlaps,0.0f); + + float p_static = std::max(overlap_same_prob*bg_overlaps-p_moving-p_dynamic,0.0f); leftover = 1.0f-p_moving-p_dynamic-p_static; float notMoving = current_overlaps; - float p_moving_leftover = 0;//0.25*leftover;//*(1-notMoving); //(1-notMoving)*leftover/4.0; - float p_dynamic_leftover = 0;//0.25*leftover;//0.5*leftover*notMoving + 0.5*leftover*(1-notMoving); //(1-notMoving)*leftover/4.0; - float p_static_leftover = 0;//0.5*leftover;//*notMoving; + float p_moving_leftover = 0.5*leftover*(1-notMoving); + float p_dynamic_leftover = 0.5*leftover; + float p_static_leftover = 0.5*leftover*notMoving; + + prior_moving = (1.0-4.0*minprob-bias)*((1.0-prob_behind)*(p_moving+p_moving_leftover) +0.25*prob_behind)+ minprob; + prior_dynamic = (1.0-4.0*minprob-bias)*((1.0-prob_behind)*(p_dynamic+p_dynamic_leftover) +0.25*prob_behind)+ minprob; + prior_static = (1.0-4.0*minprob-bias)*((1.0-prob_behind)*(p_static+p_static_leftover) +0.50*prob_behind)+ 2.0*minprob + bias; + + foreground_weight = -log(prior_moving+prior_dynamic); + background_weight = -log(prior_static); + } + } + + if(method == 2){ + if(valid){ + + float minprob = 0.1; + float bias = 0.00; + float overlap_same_prob = 0.7; + + //Prob behind all bg + float prob_behind = 1-(bg_overlaps+bg_occlusions); + float leftover = 1-prob_behind; + + float p_moving_or_dynamic = std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f); + float p_moving = 0.5f*p_moving_or_dynamic + current_occlusions; + float p_dynamic = 0.5f*p_moving_or_dynamic + std::max((bg_occlusions-current_occlusions) * current_overlaps,0.0f); + + float p_static = std::max(overlap_same_prob*bg_overlaps-p_moving-p_dynamic,0.0f); + + leftover = 1.0f-p_moving-p_dynamic-p_static; + float notMoving = current_overlaps; + + float p_moving_leftover = 0.5*leftover*(1-notMoving); + float p_dynamic_leftover = 0.5*leftover; + float p_static_leftover = 0.5*leftover*notMoving; prior_moving = p_moving+p_moving_leftover; - prior_dynamic = p_dynamic+p_dynamic_leftover; - prior_static = p_static+p_static_leftover; + prior_dynamic = (1.0-prob_behind)*(p_dynamic+p_dynamic_leftover); + prior_static = (1.0-prob_behind)*(p_static+p_static_leftover); - double p_fg = std::min( 1.0f-maxdiff ,std::max( maxdiff , prior_moving+prior_dynamic-0.01f)); - double p_bg = std::min( 1.0f-maxdiff ,std::max( maxdiff , prior_static+0.01f)); - double norm = p_fg + p_bg; - p_fg /= norm; - p_bg /= norm; + double norm = prior_moving+prior_dynamic+prior_static; + prior_dynamic += (1.0-norm)*0.5; + prior_static += (1.0-norm)*0.5; + // prior_moving = (1.0-3.0*minprob-bias)*prior_moving + minprob; + // prior_dynamic = (1.0-3.0*minprob-bias)*prior_dynamic + minprob; + // prior_static = (1.0-3.0*minprob-bias)*prior_static + minprob + bias; - if(norm > 0){ - foreground_weight = -log(p_fg); - background_weight = -log(p_bg); - } + // prior_moving = (1.0-3.0*minprob-bias)*prior_moving + minprob; + // prior_dynamic = (1.0-3.0*minprob-bias)*prior_dynamic + minprob; + // prior_static = (1.0-3.0*minprob-bias)*prior_static + minprob + bias; + + double prob_fg = prior_dynamic; + double prob_bg = prior_moving+prior_static; + //double norm = prob_fg+prob_bg; + + foreground_weight = -log((1.0-2.0*minprob)*prob_fg+minprob);//+norm*0.5); + background_weight = -log((1.0-2.0*minprob)*prob_bg+minprob);//+norm*0.5); + + // prior_moving += norm/3.0; + // prior_dynamic += norm/3.0; + // prior_static += norm/3.0; + } + } + + if(method == 3){ + if(valid){ + float minprob = 0.01; + float bias = 0.005; + float overlap_same_prob = 0.75; + + //Prob behind all bg + float prob_behind = 1-(bg_overlaps+bg_occlusions); + + float p_moving_or_dynamic = std::max((bg_occlusions-current_occlusions)*(1.0f-current_overlaps),0.0f); + float p_moving = 0.5f*p_moving_or_dynamic + current_occlusions; + float p_dynamic = 0.5f*p_moving_or_dynamic + std::max((bg_occlusions-current_occlusions) * current_overlaps,0.0f); + + float p_static = std::max(overlap_same_prob*bg_overlaps-p_moving-p_dynamic,0.0f); + + float leftover = 1.0f-p_moving-p_dynamic-p_static; + float notMoving = 1.0f-current_occlusions;//current_overlaps; + + float p_moving_leftover = 0.5*leftover*(1-notMoving); + float p_dynamic_leftover = 0.5*leftover; + float p_static_leftover = 0.5*leftover*notMoving; + + prior_moving = p_moving+p_moving_leftover; + prior_dynamic = (1.0-prob_behind)*(p_dynamic+p_dynamic_leftover); + prior_static = (1.0-prob_behind)*(p_static+p_static_leftover); + + float p_dynamic_or_static = 1.0f-p_moving-p_dynamic-p_static; + prior_dynamic += 0.5*p_dynamic_or_static; + prior_static += 0.5*p_dynamic_or_static; + + foreground_weight = -log((1.0-2.0*minprob-bias)*(prior_moving+prior_dynamic) + minprob ); + background_weight = -log((1.0-2.0*minprob-bias)* prior_static + minprob + bias); + + prior_moving = (1.0-3.0*minprob-bias)* prior_moving + minprob; + prior_dynamic = (1.0-3.0*minprob-bias)* prior_dynamic + minprob; + prior_static = (1.0-3.0*minprob-bias)* prior_static + minprob + bias; }else{ - prior_moving = 0; - prior_dynamic = 0.5; - prior_static = 0.5; + prior_static = prior_dynamic = prior_moving = 1.0/3.0; + foreground_weight = -log(0.5); + background_weight = -log(0.5); } } return 0; @@ -2310,7 +2395,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s interframe_connectionId.resize(tot_nr_pixels); interframe_connectionStrength.resize(tot_nr_pixels); - int current_point = 0; float * priors = new float[3*tot_nr_pixels]; float * prior_weights = new float[2*tot_nr_pixels]; @@ -2354,28 +2438,24 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for(unsigned int i = 0; i < dvec.size(); i++){dstdval += dvec[i]*dvec[i];} dstdval = sqrt(dstdval/double(dvec.size()-1)); - DistanceWeightFunction2PPR2 * dfuncTMP = new DistanceWeightFunction2PPR2(); + GeneralizedGaussianDistribution * dggdnfunc = new GeneralizedGaussianDistribution(true,true,false,true,true); + dggdnfunc->nr_refineiters = 4; + DistanceWeightFunction2PPR3 * dfuncTMP = new DistanceWeightFunction2PPR3(dggdnfunc); dfunc = dfuncTMP; - dfuncTMP->refine_mean = true; - dfuncTMP->zeromean = false; - dfuncTMP->startreg = 0.0; - //dfuncTMP->startreg = 0.001; - //dfuncTMP->startreg = 0.0000001; + dfuncTMP->startreg = 0.00; dfuncTMP->debugg_print = false; dfuncTMP->bidir = true; - //dfuncTMP->bidir = false; - //dfuncTMP->maxp = 0.9; - dfuncTMP->maxp = 0.9; + dfuncTMP->zeromean = false; + dfuncTMP->maxp = 0.9999; dfuncTMP->maxd = 0.5; dfuncTMP->histogram_size = 1000; - dfuncTMP->fixed_histogram_size = false;//true; - dfuncTMP->max_under_mean = false; + dfuncTMP->fixed_histogram_size = false; dfuncTMP->startmaxd = dfuncTMP->maxd; dfuncTMP->starthistogram_size = dfuncTMP->histogram_size; dfuncTMP->blurval = 0.5; - dfuncTMP->stdval2 = dstdval; dfuncTMP->maxnoise = dstdval; dfuncTMP->compute_infront = true; + dfuncTMP->ggd = true; dfuncTMP->reset(); dfunc->computeModel(dvec); @@ -2387,7 +2467,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s nfuncTMP->debugg_print = false; nfuncTMP->bidir = false; nfuncTMP->zeromean = true; - nfuncTMP->maxp = 0.999; + nfuncTMP->maxp = 0.9999; nfuncTMP->maxd = 1.0; nfuncTMP->histogram_size = 1000; nfuncTMP->fixed_histogram_size = true; @@ -2413,8 +2493,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s double total_dealloctime = 0; double total_Dynw = 0; - float bias = 0.0001; - float maxdiff = bias+0.0001; double maxprob_same = 0.999999999999999999; for(unsigned int i = 0; i < frames.size(); i++){ @@ -2472,8 +2550,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } for(unsigned int j = 0; j < nr_pixels; j++){ - bg_occlusions[j] = std::min(0.999,bg_occlusions[j]/std::max(1.0,bg_notocclusions[j])); - current_occlusions[j] = std::min(0.999,current_occlusions[j]/std::max(1.0,current_notocclusions[j])); + bg_occlusions[j] = std::min(0.99999,bg_occlusions[j]/std::max(1.0,bg_notocclusions[j])); + current_occlusions[j] = std::min(0.99999,current_occlusions[j]/std::max(1.0,current_notocclusions[j])); } total_Dynw += getTime()-startTime; @@ -2481,61 +2559,15 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s cv::Mat priorsimg; priorsimg.create(height,width,CV_32FC3); float * priorsdata = (float*)priorsimg.data; - //for(int i = 0; i < 3*nr_pixels; i++){priorsdata[i] = priors[3*offset+i];} startTime = getTime(); unsigned char * detdata = (unsigned char*)(frame->det_dilate.data); - float bias = 0.0001; for(unsigned int h = 0; h < height;h++){ for(unsigned int w = 0; w < width;w++){ int ind = h*width+w; valids[offset+ind] = detdata[ind] == 0 && normalsdata[3*ind] != 2; - -/* - if(valids[offset+ind]){ - -// float p_moving = current_occlusions[ind]; -// float p_dynamic = std::max((bg_occlusions[ind]-current_occlusions[ind]) * current_overlaps[ind],0.0); -// p_dynamic += 0.5*std::max((bg_occlusions[ind]-current_occlusions[ind])*(1-current_overlaps[ind]),0.0); -// p_moving += 0.5*std::max((bg_occlusions[ind]-current_occlusions[ind])*(1-current_overlaps[ind]),0.0); - - float p_moving_or_dynamic = std::max((bg_occlusions[ind]-current_occlusions[ind])*(1-current_overlaps[ind]),0.0); - float p_moving = 0.5*p_moving_or_dynamic + current_occlusions[ind]; - float p_dynamic = 0.5*p_moving_or_dynamic + std::max((bg_occlusions[ind]-current_occlusions[ind]) * current_overlaps[ind],0.0); - float p_static = std::max(bg_overlaps[ind]-p_moving-p_dynamic,0.0); - - float leftover = 1-p_moving-p_dynamic-p_static; - float notMoving = current_overlaps[ind]; - - float p_moving_leftover = 0.5*leftover*(1-notMoving); //(1-notMoving)*leftover/4.0; - float p_dynamic_leftover = 0.5*leftover*notMoving + 0.5*leftover*(1-notMoving); //(1-notMoving)*leftover/4.0; - float p_static_leftover = 0.5*leftover*notMoving; - - priors[3*(offset+ind)+0] = p_moving+p_moving_leftover; - priors[3*(offset+ind)+1] = p_dynamic+p_dynamic_leftover; - priors[3*(offset+ind)+2] = p_static+p_static_leftover; - - - - }else{ - if(detdata[ind] == 0){ - priorsdata[3*ind+2] = 1.0; - priorsdata[3*ind+1] = 0.0; - priorsdata[3*ind+0] = 1.0; - }else{ - priorsdata[3*ind+2] = 1.0; - priorsdata[3*ind+1] = 1.0; - priorsdata[3*ind+0] = 0.0; - } - - priors[3*(offset+ind)+0] = 0; - priors[3*(offset+ind)+1] = 0.5; - priors[3*(offset+ind)+2] = 0.5; - } -*/ - - setupPriors(1, + setupPriors(3, current_occlusions[ind],current_overlaps[ind],bg_occlusions[ind],bg_overlaps[ind],valids[offset+ind], priors[3*(offset+ind)+0], priors[3*(offset+ind)+1],priors[3*(offset+ind)+2], prior_weights[2*(offset+ind)+0], prior_weights[2*(offset+ind)+1]); @@ -2555,29 +2587,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s gc::Graph * g = new gc::Graph(nr_pixels,2*nr_pixels); for(unsigned long ind = 0; ind < nr_pixels;ind++){ g -> add_node(); -// double p_fg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*(offset+ind)+1]))-bias; -// double p_bg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*(offset+ind)+2] + priors[3*(offset+ind)+0]))+bias; -// double norm = p_fg + p_bg; -//// p_fg /= norm; -//// p_bg /= norm; -// p_fg /= 2*norm; -// p_bg /= 2*norm; -// p_fg += 0.25; -// p_bg += 0.25; - -// if(norm <= 0){ -// g -> add_tweights( ind, 0, 0 ); -// continue; -// } - - -// double weightFG = -log(p_fg); -// double weightBG = -log(p_bg); - double weightFG = prior_weights[2*(offset+ind)+0]; double weightBG = prior_weights[2*(offset+ind)+1]; - - g -> add_tweights( ind, weightFG, weightBG ); } @@ -2607,64 +2618,44 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s if(debugg != 0){printf("local inference time: %10.10fs\n\n",getTime()-start_inf);} - -// cv::Mat edgeimgX; -// edgeimgX.create(height,width,CV_32FC3); -// float * edgedataX = (float*)edgeimgX.data; - -// cv::Mat edgeimgY; -// edgeimgY.create(height,width,CV_32FC3); -// float * edgedataY = (float*)edgeimgY.data; - -// for(int j = 0; j < width*height; j++){ -// edgedataX[3*j+0] = 0; -// edgedataX[3*j+1] = 1-probs[0][j]; -// edgedataX[3*j+2] = probs2[0][j]; - -// edgedataY[3*j+0] = 0; -// edgedataY[3*j+1] = 1-probs[1][j]; -// edgedataY[3*j+2] = probs2[1][j]; -// } - - - cv::Mat edgeimg; - edgeimg.create(height,width,CV_32FC3); - float * edgedata = (float*)edgeimg.data; - - for(int j = 0; j < width*height; j++){ - edgedata[3*j+0] = 0; - edgedata[3*j+1] = 1-probs[0][j]; - edgedata[3*j+2] = 1-probs[1][j]; - } - - cv::Mat labelimg; - labelimg.create(height,width,CV_32FC3); - float * labelimgdata = (float*)labelimg.data; - for(unsigned long ind = 0; ind < nr_pixels;ind++){ - double label = g->what_segment(ind); - labelimgdata[3*ind+0] = 0; - labelimgdata[3*ind+1] = label; - labelimgdata[3*ind+2] = 1-label; - } - - cv::Mat detrgb; - detrgb.create(height,width,CV_8UC3); - - for(unsigned long ind = 0; ind < nr_pixels;ind++){ - int dd = detdata[ind] == 0; - detrgb.data[3*ind+0] = dd * frame->rgb.data[3*ind+0]; - detrgb.data[3*ind+1] = dd * frame->rgb.data[3*ind+1]; - detrgb.data[3*ind+2] = dd * frame->rgb.data[3*ind+2]; - } - - cv::namedWindow( "edgeimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "edgeimg", edgeimg ); - cv::namedWindow( "det_dilate" , cv::WINDOW_AUTOSIZE ); cv::imshow( "det_dilate", frame->det_dilate); - cv::namedWindow( "det_dilate2" , cv::WINDOW_AUTOSIZE ); cv::imshow( "det_dilate2", detrgb); - cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb ); - cv::namedWindow( "depth" , cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", frame->depth ); - cv::namedWindow( "priors" , cv::WINDOW_AUTOSIZE ); cv::imshow( "priors", priorsimg ); - cv::namedWindow( "labelimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "labelimg", labelimg ); - cv::waitKey(0); + // cv::Mat edgeimg; + // edgeimg.create(height,width,CV_32FC3); + // float * edgedata = (float*)edgeimg.data; + + // for(int j = 0; j < width*height; j++){ + // edgedata[3*j+0] = 0; + // edgedata[3*j+1] = 1-probs[0][j]; + // edgedata[3*j+2] = 1-probs[1][j]; + // } + + // cv::Mat labelimg; + // labelimg.create(height,width,CV_32FC3); + // float * labelimgdata = (float*)labelimg.data; + // for(unsigned long ind = 0; ind < nr_pixels;ind++){ + // double label = g->what_segment(ind); + // labelimgdata[3*ind+0] = 0; + // labelimgdata[3*ind+1] = label; + // labelimgdata[3*ind+2] = 1-label; + // } + + // cv::Mat detrgb; + // detrgb.create(height,width,CV_8UC3); + + // for(unsigned long ind = 0; ind < nr_pixels;ind++){ + // int dd = detdata[ind] == 0; + // detrgb.data[3*ind+0] = dd * frame->rgb.data[3*ind+0]; + // detrgb.data[3*ind+1] = dd * frame->rgb.data[3*ind+1]; + // detrgb.data[3*ind+2] = dd * frame->rgb.data[3*ind+2]; + // } + + // cv::namedWindow( "edgeimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "edgeimg", edgeimg ); + // cv::namedWindow( "det_dilate" , cv::WINDOW_AUTOSIZE ); cv::imshow( "det_dilate", frame->det_dilate); + // cv::namedWindow( "det_dilate2" , cv::WINDOW_AUTOSIZE ); cv::imshow( "det_dilate2", detrgb); + // cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frame->rgb ); + // cv::namedWindow( "depth" , cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", frame->depth ); + // cv::namedWindow( "priors" , cv::WINDOW_AUTOSIZE ); cv::imshow( "priors", priorsimg ); + // cv::namedWindow( "labelimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "labelimg", labelimg ); + // cv::waitKey(0); @@ -2718,24 +2709,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s gc::Graph * g = new gc::Graph(current_point,frameConnections+interframeConnections); for(unsigned long i = 0; i < current_point;i++){ g -> add_node(); + double weightFG = prior_weights[2*i+0]; + double weightBG = prior_weights[2*i+1]; - double p_fg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*i+1]))-bias; - double p_bg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*i+2] + priors[3*i+0]))+bias; - double norm = p_fg + p_bg; - p_fg /= 2*norm; - p_bg /= 2*norm; - p_fg += 0.25; - p_bg += 0.25; - - if(norm <= 0){ - g -> add_tweights( i, 0, 0 ); - continue; - } - - - - double weightFG = -log(p_fg); - double weightBG = -log(p_bg); g -> add_tweights( i, weightFG, weightBG ); } @@ -2867,36 +2843,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s double end_inf = getTime(); printf("static inference time: %10.10fs interfrace_constraints added ratio: %f\n",end_inf-start_inf,interfrace_constraints_added/double(interframeConnections)); - if(false){ - printf("3-class\n"); - for(unsigned int i = 0; i < current_point; i++){ - cloud->points[i].r = priors[3*i+0]*255.0; - cloud->points[i].g = priors[3*i+1]*255.0; - cloud->points[i].b = priors[3*i+2]*255.0; - } - - pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); - cloud_sample->points.clear(); - for(unsigned int i = 0; i < current_point; i++){ - cloud_sample->points.push_back(cloud->points[i]); - } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); - viewer->spin(); - - for(unsigned int i = 0; i < current_point; i++){ - cloud_sample->points[i].r = 255*labels[i]; - cloud_sample->points[i].g = 255*(1-labels[i]); - cloud_sample->points[i].b = 0; - } - - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); - viewer->spin(); - } - - double start_inf1 = getTime(); - const unsigned int nr_frames = frames.size(); const unsigned int width = frames[0]->camera->width; const unsigned int height = frames[0]->camera->height; @@ -2922,7 +2868,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s unsigned long todocounter = 0; std::vector< unsigned long > todo; todo.push_back(ind); - double score = 0; + + double score0 = 0; + double score1 = 0; double totsum = 0; while(todocounter < todo.size()){ unsigned long cind = todo[todocounter++]; @@ -2938,10 +2886,16 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s double p2 = priors[3*cind+2]; if(valids[cind]){ - double s = 0; - if(p0 > p2){s += p1 - p0;} - else{ s += p1 - p2;} - score += s; + double s0 = 0; + if(p1 > p2){s0 += p0 - p1;} + else{ s0 += p0 - p2;} + score0 += s0; + + double s1 = 0; + if(p0 > p2){s1 += p1 - p0;} + else{ s1 += p1 - p2;} + score1 += s1; + totsum++; } @@ -2983,57 +2937,21 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } - if(debugg != 0){printf("score: %f\n",score);} - - labelID.push_back(0); - if(score < 200){continue;} - - if(false && debugg && todo.size() > 10000){ - for(unsigned int i = 0; i < current_point; i++){ - cloud->points[i].r = priors[3*i+0]*255.0; - cloud->points[i].g = priors[3*i+1]*255.0; - cloud->points[i].b = priors[3*i+2]*255.0; - } - - pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); - cloud_sample->points.clear(); - for(unsigned int i = 0; i < current_point; i++){ - cloud_sample->points.push_back(cloud->points[i]); - } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); - viewer->spin(); - - for(unsigned int i = 0; i < current_point; i++){ - cloud_sample->points[i].r = 255; - cloud_sample->points[i].g = 0; - cloud_sample->points[i].b = 0; - } + if(debugg != 0){printf("score0: %f score1: %f\n",score0,score1);} + if(std::max(score0,score1) < 100){continue;} - for(unsigned int j = 0; j < todo.size(); j++){ - unsigned int i = todo[j]; - cloud_sample->points[i].r = 0; - cloud_sample->points[i].g = 255; - cloud_sample->points[i].b = 0; - } - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); - viewer->spin(); - } - - if(current_label == 1){ + if(score1 > score0){ labelID.back() = ++nr_obj_dyn; - if(debugg != 0){printf("Dynamic: %f -> %f\n",score,totsum);} + if(debugg != 0){printf("Dynamic: %f -> %f\n",score1,totsum);} sr.component_dynamic.push_back(todo); - sr.scores_dynamic.push_back(score); + sr.scores_dynamic.push_back(score1); sr.total_dynamic.push_back(totsum); - } - if(current_label == 2){ + }else{ labelID.back() = --nr_obj_mov; - if(debugg != 0){printf("Moving: %f -> %f\n",score,totsum);} + if(debugg != 0){printf("Moving: %f -> %f\n",score0,totsum);} sr.component_moving.push_back(todo); - sr.scores_moving.push_back(score); + sr.scores_moving.push_back(score0); sr.total_moving.push_back(totsum); } } @@ -3083,42 +3001,42 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dynmask.push_back(d); -// unsigned short * depthdata = (unsigned short *)(frames[i]->depth.data); -// cv::Mat debuggimg; -// debuggimg.create(height,width,CV_8UC3); -// unsigned char * debuggimgdata = (unsigned char*)debuggimg.data; -// cv::Mat edgeimg; -// edgeimg.create(height,width,CV_32FC3); -// float * edgedata = (float*)edgeimg.data; -// for(unsigned int label = 0; label < labelID.size(); label++){ -// int lid = labelID[label]; -// printf("lid: %i\n",lid); -// if(lid <= 0){continue;} -// bool hasdata = false; -// for(int j = 0; j < width*height; j++){ -// if(ddata[j] == lid){ -// debuggimgdata[3*j+0] = 255; -// debuggimgdata[3*j+1] = 255; -// debuggimgdata[3*j+2] = 255; -// hasdata = true; -// }else{ -// debuggimgdata[3*j+0] = 0; -// debuggimgdata[3*j+1] = 0; -// debuggimgdata[3*j+2] = 0; -// } -// if(depthdata[j] == 0){ -// debuggimgdata[3*j+0] = 255; -// debuggimgdata[3*j+1] = 0; -// debuggimgdata[3*j+2] = 255; -// } -// } -// if(hasdata){ -// cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frames[i]->rgb ); -// cv::namedWindow( "debuggimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "debuggimg", debuggimg ); -// cv::namedWindow( "edgeimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "edgeimg", edgeimg ); -// cv::waitKey(0); -// } -// } + // unsigned short * depthdata = (unsigned short *)(frames[i]->depth.data); + // cv::Mat debuggimg; + // debuggimg.create(height,width,CV_8UC3); + // unsigned char * debuggimgdata = (unsigned char*)debuggimg.data; + // cv::Mat edgeimg; + // edgeimg.create(height,width,CV_32FC3); + // float * edgedata = (float*)edgeimg.data; + // for(unsigned int label = 0; label < labelID.size(); label++){ + // int lid = labelID[label]; + // printf("lid: %i\n",lid); + // if(lid <= 0){continue;} + // bool hasdata = false; + // for(int j = 0; j < width*height; j++){ + // if(ddata[j] == lid){ + // debuggimgdata[3*j+0] = 255; + // debuggimgdata[3*j+1] = 255; + // debuggimgdata[3*j+2] = 255; + // hasdata = true; + // }else{ + // debuggimgdata[3*j+0] = 0; + // debuggimgdata[3*j+1] = 0; + // debuggimgdata[3*j+2] = 0; + // } + // if(depthdata[j] == 0){ + // debuggimgdata[3*j+0] = 255; + // debuggimgdata[3*j+1] = 0; + // debuggimgdata[3*j+2] = 255; + // } + // } + // if(hasdata){ + // cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frames[i]->rgb ); + // cv::namedWindow( "debuggimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "debuggimg", debuggimg ); + // cv::namedWindow( "edgeimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "edgeimg", edgeimg ); + // cv::waitKey(0); + // } + // } } if(debugg){ @@ -3144,11 +3062,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for(unsigned int i = 0; i < current_point; i++){ if(rand() % 4 == 0){ - double p_fg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*i+1]))-bias; - double p_bg = std::min( 1.0f-maxdiff ,std::max( maxdiff , priors[3*i+2] + priors[3*i+0]))+bias; - double norm = p_fg + p_bg; - p_fg /= norm; - p_bg /= norm; + double p_fg = exp(-prior_weights[2*i+0]); cloud_sample->points.push_back(cloud->points[i]); cloud_sample->points.back().r = p_fg*255.0;//(priors[3*i+0]+priors[3*i+2])*255.0; @@ -3177,9 +3091,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s int randb = rand()%256; for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){ cloud_sample->points.push_back(cloud->points[sr.component_dynamic[c][i]]); - cloud_sample->points.back().r = randr; - cloud_sample->points.back().g = randg; - cloud_sample->points.back().b = randb; + cloud_sample->points.back().r = 0;//randr; + cloud_sample->points.back().g = 255;//randg; + cloud_sample->points.back().b = 0;//randb; } } @@ -3189,9 +3103,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s int randb = rand()%256; for(unsigned int i = 0; i < sr.component_moving[c].size(); i++){ cloud_sample->points.push_back(cloud->points[sr.component_moving[c][i]]); - cloud_sample->points.back().r = randr; - cloud_sample->points.back().g = randg; - cloud_sample->points.back().b = randb; + cloud_sample->points.back().r = 255;//randr; + cloud_sample->points.back().g = 0;//randg; + cloud_sample->points.back().b = 0;//randb; } } viewer->removeAllPointClouds(); @@ -3735,6 +3649,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s if(weight > 1 && g->what_segment(i2) != g->what_segment(j2)){ diffs++; } + dfuncTMP->compute_infront = true; } } } diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp index 3ad29bad..cdb1651c 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp @@ -184,8 +184,15 @@ void DistanceWeightFunction2PPR3::recomputeProbs(){ double hs = std::max(minhist,std::max(1.0,double(blur_histogram[k]))); prob[k] = std::min(maxp , noise[k]/hs);//never fully trust any data } - infront[k] = (1-prob[k])*noisecdf[k]; + infront[k] = (1-prob[k])*noisecdf[k]; } + + if(false && debugg_print){ + for(int k = 0; k < histogram_size; k+=10){ + printf("%i -> hist: %f prob[k]: %f noisecdf[k]: %f infront[k]: %f\n",k,double(blur_histogram[k]),prob[k],noisecdf[k],infront[k]); + } + } + //exit(0); } void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){ @@ -196,23 +203,33 @@ void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){ if(debugg_print){printf("\n%%################################################### ITER:%i ############################################################\n",iter);} if(!fixed_histogram_size){ if(first){ - first = false; + double sum = 0; for(unsigned int j = 0; j < nr_data; j++){ for(int k = 0; k < nr_dim; k++){sum += mat(k,j)*mat(k,j);} } double stdval = sqrt(sum / double(nr_data*nr_dim)); if(debugg_print){printf("%%stdval: %f\n",stdval);} - dist->setNoise(stdval); + histogram_mul2 = double(histogram_size)/(maxd-mind); + dist->setNoise(stdval*histogram_mul2); } if(update_size){ + + if(debugg_print){printf("%%update_size\n");dist->print();} double next_maxd; double next_mind; start_time = getTime(); - dist->getMaxdMind(next_maxd,next_mind,0.001); - next_maxd *= (maxd-mind)/double(histogram_size); - next_mind *= (maxd-mind)/double(histogram_size); + dist->getMaxdMind(next_maxd,next_mind,0.0000001); +// next_maxd *= (maxd-mind)/double(histogram_size); +// next_mind *= (maxd-mind)/double(histogram_size); + if(first){ + next_maxd *= (maxd-mind)/double(histogram_size); + next_mind = -next_maxd; + }else { + next_maxd = getDfromInd(next_maxd); + next_mind = getDfromInd(next_mind); + } if(debugg_print){printf("%%getMaxdMind time: %5.5fs\n",getTime()-start_time);} if(!bidir){next_mind = 0;} mind = next_mind; @@ -233,6 +250,7 @@ void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){ blurval = blur*double(histogram_size)*float(histogram_size)/float(new_histogram_size); if(debugg_print){printf("%%nr_inside: %f histogram_size: %i blurval: %f\n",nr_inside,histogram_size,blurval);} } + first = false; } if(debugg_print){printf("%%histogram_size: %5.5f maxd: %5.5f mind: %5.5f\n",double(histogram_size),maxd,mind);} @@ -264,7 +282,8 @@ void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){ if(debugg_print){printf("noisecdf = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",int(noisecdf[k]));} printf("];\n");} if(debugg_print){printf("prob = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",prob[k]);} printf("];\n");} if(debugg_print){printf("infront = ["); for(int k = 0; k < 3000 && k < histogram_size; k++){printf("%2.2f ",infront[k]);} printf("];\n");} - if(debugg_print){printf("clf; hold on; plot(hist_smooth,'r','LineWidth',2); plot(hist,'m','LineWidth',2); plot(noise,'b','LineWidth',2); plot(prob*max(noise),'g','LineWidth',2); plot(infront*max(noise),'g','LineWidth',2);\n");} + if(debugg_print){printf("figure(%i);",iter+1);} + if(debugg_print){printf("clf; hold on; plot(hist_smooth,'r','LineWidth',2); plot(hist,'m','LineWidth',2); plot(noise,'b','LineWidth',2); plot(prob*max(noise),'g','LineWidth',2); plot(infront*max(noise),'g','LineWidth',2);\n");} if(!fixed_histogram_size && update_size ){ @@ -272,15 +291,23 @@ void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){ double next_mind; start_time = getTime(); - dist->getMaxdMind(next_maxd,next_mind,0.001); - next_maxd *= (maxd-mind)/double(histogram_size); - next_mind *= (maxd-mind)/double(histogram_size); + dist->getMaxdMind(next_maxd,next_mind,0.0000001); + if(debugg_print){ printf("%%getMaxdMind time: %5.5fs\n",getTime()-start_time); printf("%%current: %f %f\n",mind,maxd); printf("%%next: %f %f\n",next_mind,next_maxd); } + next_maxd = getDfromInd(next_maxd); + next_mind = getDfromInd(next_mind); + //next_maxd *= (maxd-mind)/double(histogram_size); + //next_mind *= (maxd-mind)/double(histogram_size); + + if(debugg_print){ + printf("%%next: %f %f\n",next_mind,next_maxd); + } + if(!bidir){next_mind = 0;} //Ensure overlap @@ -299,7 +326,7 @@ void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){ double newlogdiff = log(ratio); if(debugg_print){printf("%%overlap: %5.5f ratio: %5.5f newlogdiff: %5.5f\n",overlap,ratio,newlogdiff);} - if(fabs(newlogdiff) > 0.2 && iter < 3){ + if(fabs(newlogdiff) > 0.05 && iter < 5){ iter++; computeModel(mat); return; @@ -441,11 +468,19 @@ double DistanceWeightFunction2PPR3::getConvergenceThreshold(){ } inline double DistanceWeightFunction2PPR3::getInd(double d, bool debugg){ - if(bidir){ - return 0.00001+(d-mind)*histogram_mul2; - }else{ - return 0.00001+fabs(d)*histogram_mul2; - } + if(bidir){ + return 0.00001+(d-mind)*histogram_mul2; + }else{ + return 0.00001+fabs(d)*histogram_mul2; + } +} + +inline double DistanceWeightFunction2PPR3::getDfromInd(double ind, bool debugg){ + if(bidir){ + return ind/histogram_mul2+mind; + }else{ + return ind/histogram_mul2; + } } } diff --git a/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp index dbcc2569..26be398b 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/Distribution.cpp @@ -2,7 +2,9 @@ namespace reglib { -Distribution::Distribution(){} +Distribution::Distribution(){ + debugg_print = false; +} Distribution::~Distribution(){} void Distribution::reset(){printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__);} void Distribution::train(std::vector & hist, unsigned int nr_bins){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} @@ -14,7 +16,7 @@ void Distribution::setRegularization(double x){regularization = x;update();} double Distribution::getNoise(){return 1;} void Distribution::setNoise(double x){ printf("%s in %s not implemented, stopping\n",__PRETTY_FUNCTION__,__FILE__);exit(0);} void Distribution::getMaxdMind(double & maxd, double & mind, double prob){ - printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__); + if(debugg_print){printf("%s in %s\n",__PRETTY_FUNCTION__,__FILE__);} double half = prob*0.5; double minDist = 0; double minDistScore = getcdf(mean+minDist); diff --git a/quasimodo/quasimodo_models/src/weightfunctions/GaussianDistribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/GaussianDistribution.cpp index 622c95f4..40e99f91 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/GaussianDistribution.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/GaussianDistribution.cpp @@ -16,6 +16,7 @@ GaussianDistribution::GaussianDistribution(bool refine_std_, bool zeromean_, boo mean = mean_; stdval = stdval_; update(); + debugg_print = false; } //GaussianDistribution::GaussianDistribution(double mul_, double mean_, double stdval_){ // mul = mul_; diff --git a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp index 5eb89329..b77c82e3 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp @@ -18,6 +18,7 @@ GeneralizedGaussianDistribution::GeneralizedGaussianDistribution(bool refine_std stdval = stdval_; power = power_; update(); + debugg_print = false; } GeneralizedGaussianDistribution::~GeneralizedGaussianDistribution(){} @@ -178,7 +179,7 @@ void GeneralizedGaussianDistribution::setNoise(double x){ } void GeneralizedGaussianDistribution::print(){ - printf("GeneralizedGaussianDistribution:: mul = %5.5f mean = %5.5f stdval = %5.5f reg = %5.5f power = %5.5f\n",mul,mean,stdval,regularization,power); + printf("%%GeneralizedGaussianDistribution:: mul = %5.5f mean = %5.5f stdval = %5.5f reg = %5.5f power = %5.5f\n",mul,mean,stdval,regularization,power); } double GeneralizedGaussianDistribution::getNoise(){return stdval+regularization;} From b92a577e9a42fbd6eb380078ab01a0b36a2559e9 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 31 Oct 2016 06:30:43 +0100 Subject: [PATCH 174/255] added visuals --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 4 +- quasimodo/quasimodo_brain/src/Util/Util.h | 2 +- .../metaroom_additional_view_processing.cpp | 39 +++++---- .../quasimodo_models/include/core/RGBDFrame.h | 5 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 81 ++++++++++++++++--- 5 files changed, 103 insertions(+), 28 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 33bc3174..78bcbe6e 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -249,7 +249,7 @@ Eigen::Matrix4d getMat(tf::StampedTransform tf){ return epose.matrix(); } -reglib::Model * load_metaroom_model(std::string sweep_xml){ +reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath){ int slash_pos = sweep_xml.find_last_of("/"); std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; printf("load_metaroom_model(%s)\n",sweep_folder.c_str()); @@ -281,7 +281,7 @@ reglib::Model * load_metaroom_model(std::string sweep_xml){ //cam->print(); Eigen::Matrix4d m = m2*getMat(roomData.vIntermediateRoomCloudTransformsRegistered[i]); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, m); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, m,true,savePath); current_room_frames.push_back(frame); if(sweepmodel == 0){ diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index 4b62c7a3..d64f580e 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -53,7 +53,7 @@ quasimodo_msgs::model getModelMSG(reglib::Model * model, bool addClouds = false) std::vector getRegisteredViewPoses(const std::string& poses_file, const int& no_transforms); Eigen::Matrix4d getMat(tf::StampedTransform tf); -reglib::Model * load_metaroom_model(std::string sweep_xml); +reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath = ""); void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, int debugg = 0); std::vector loadModelsXML(std::string path); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 472c693a..5e0b21b5 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -134,6 +134,7 @@ std::vector< ros::Publisher > m_PublisherStatuss; std::vector< ros::Publisher > out_pubs; std::vector< ros::Publisher > model_pubs; +std::string saveVisuals = ""; std::string posepath = "testposes.xml"; std::vector sweepPoses; @@ -378,7 +379,7 @@ int readNumberOfViews(std::string xmlFile){ return count; } -void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses, bool compute_edges = true){ +void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses, bool compute_edges = true, std::string savePath = ""){ QFile file(xmlFile.c_str()); @@ -497,7 +498,7 @@ void readViewXML(std::string xmlFile, std::vector & frames, token = xmlReader->readNext();//Pose elementName = xmlReader->name().toString(); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, regpose); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, regpose,true,savePath); frames.push_back(frame); poses.push_back(pose); } @@ -528,7 +529,7 @@ void setBaseSweep(std::string path){ } } -reglib::Model * processAV(std::string path, bool compute_edges = true){ +reglib::Model * processAV(std::string path, bool compute_edges = true, std::string savePath = ""){ printf("processAV: %s\n",path.c_str()); int slash_pos = path.find_last_of("/"); @@ -567,7 +568,7 @@ reglib::Model * processAV(std::string path, bool compute_edges = true){ } } - reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path); + reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; sweep->relativeposes[i] = sweepPoses[i]; @@ -606,7 +607,7 @@ reglib::Model * processAV(std::string path, bool compute_edges = true){ masks.push_back(new reglib::ModelMask(fullmask)); reglib::Camera * cam = sweep->frames.front()->camera->clone(); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m);//a.matrix()); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m,true,savePath);//a.matrix()); frames.push_back(frame); both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); @@ -779,7 +780,7 @@ std::vector readPoseXML(std::string xmlFile){ return poses; } -reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true){ +reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std::string saveVisuals_sp = ""){ printf("processing: %s\n",path.c_str()); if ( ! boost::filesystem::exists( path ) ){return 0;} @@ -822,13 +823,19 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true){ fullmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); } - readViewXML(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes,compute_edges); + readViewXML(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes,compute_edges,saveVisuals_sp); fullmodel->recomputeModelPoints(); }else{ - fullmodel = processAV(path,compute_edges); + fullmodel = processAV(path,compute_edges,saveVisuals_sp); writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); } + if(saveVisuals.size() > 0){ + for(unsigned int i = 0; i < fullmodel->frames.size(); i++){ + + } + } + return fullmodel; } @@ -856,7 +863,7 @@ int processMetaroom(std::string path, bool store_old_xml = true){ printf("current_roomData loaded\n"); - reglib::Model * fullmodel = getAVMetaroom(path); + reglib::Model * fullmodel = getAVMetaroom(path,true,saveVisuals); printf("fullmodel->frames.size() = %i\n",fullmodel->frames.size()); @@ -1649,8 +1656,9 @@ int main(int argc, char** argv){ else if(std::string(argv[i]).compare("-once") == 0){ once = true;} else if(std::string(argv[i]).compare("-nobase") == 0){ baseSetting = false;} else if(std::string(argv[i]).compare("-recomputeRelativePoses") == 0){ recomputeRelativePoses = true;} - else if(std::string(argv[i]).compare("-v_init") == 0){ visualization_lvl_regini = 1;inputstate = 17;} - else if(std::string(argv[i]).compare("-v_reg") == 0){ visualization_lvl_regref = 1;inputstate = 18;} + else if(std::string(argv[i]).compare("-v_init") == 0){ visualization_lvl_regini = 1;inputstate = 17;} + else if(std::string(argv[i]).compare("-v_reg") == 0){ visualization_lvl_regref = 1;inputstate = 18;} + else if(std::string(argv[i]).compare("-saveVisuals") == 0){inputstate = 19;} else if(inputstate == 0){ segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); }else if(inputstate == 1){ @@ -1688,10 +1696,11 @@ int main(int argc, char** argv){ setBaseSweep(std::string(argv[i])); }else if(inputstate == 17){ visualization_lvl_regini = atoi(argv[i]); - }else if(inputstate == 18){ - visualization_lvl_regref = atoi(argv[i]); - } - + }else if(inputstate == 18){ + visualization_lvl_regref = atoi(argv[i]); + }else if(inputstate == 19){ + saveVisuals = std::string(argv[i]); + } } if(visualization_lvl != 0 || visualization_lvl_regref != 0 || visualization_lvl_regini != 0){ diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index c75d3567..3d280f42 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -52,7 +52,7 @@ namespace reglib std::vector< std::vector > intersections; RGBDFrame(); - RGBDFrame(Camera * camera_,cv::Mat rgb_, cv::Mat depth_, double capturetime_ = 0, Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity(), bool compute_normals = true); + RGBDFrame(Camera * camera_,cv::Mat rgb_, cv::Mat depth_, double capturetime_ = 0, Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity(), bool compute_normals = true, std::string savePath = ""); ~RGBDFrame(); void show(bool stop = false); @@ -68,6 +68,9 @@ namespace reglib std::vector getSuperPoints(Eigen::Matrix4d cp = Eigen::Matrix4d::Identity(), unsigned int step = 1, bool zeroinclude = true); std::vector< std::vector > getImageProbs(bool depthonly = false); + + void saveCombinedImages(std::string path); + void saveCombinedProcessedImages(std::string path); }; } diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index e24e0b1b..c3d37806 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -614,11 +614,8 @@ RGBDFrame * RGBDFrame::clone(){ // return probs2; //} -RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals){ - - std::set myset; - - +RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals, std::string savePath){ + printf("savepath: %s\n",savePath.c_str()); bool verbose = false; if(verbose) printf("------------------------------\n"); @@ -634,6 +631,11 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt capturetime = capturetime_; pose = pose_; + if(savePath.size() != 0){ + cv::imwrite( savePath+"/rgb.png", rgb ); + cv::imwrite( savePath+"/depth.png", depth ); + } + IplImage iplimg = rgb_; IplImage* img = &iplimg; @@ -929,7 +931,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt DistanceWeightFunction2PPR2 * funcB = new DistanceWeightFunction2PPR2(); funcR->zeromean = funcG->zeromean = funcB->zeromean = true; funcR->maxp = funcG->maxp = funcB->maxp = 0.999999999999999999; - funcR->startreg = funcG->startreg = funcB->startreg = 1.0; + funcR->startreg = funcG->startreg = funcB->startreg = 0.1; funcR->debugg_print = funcG->debugg_print = funcB->debugg_print = false; funcR->maxd = funcG->maxd = funcB->maxd = 256.0; funcR->histogram_size = funcG->histogram_size = funcB->histogram_size = 256; @@ -1102,7 +1104,9 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); printf("complete time to create RGBD image: %5.5fs\n",getTime()-completeStartTime); - //getImageProbs(); + //getImageProbs(); + saveCombinedImages(""); + saveCombinedProcessedImages(""); } RGBDFrame::~RGBDFrame(){ @@ -1308,7 +1312,7 @@ RGBDFrame * RGBDFrame::load(Camera * cam, std::string path){ size = file.tellg(); buffer = new char [size]; file.seekg (0, std::ios::beg); - file.read (buffer, size); + file.read (buffer, size); file.close(); double * buffer_double = (double *)buffer; @@ -1485,7 +1489,7 @@ std::vector< std::vector > RGBDFrame::getImageProbs(bool depthonly){ for(unsigned int i = 0; i < nr_pixels;i++){ //dxc[i] = 1.0-std::max(dedata[3*i+1],cedata[3*i+1]); //dyc[i] = 1.0-std::max(dedata[3*i+2],cedata[3*i+2]); - +//ProcessedImages @@ -1516,4 +1520,63 @@ std::vector< std::vector > RGBDFrame::getImageProbs(bool depthonly){ return probs2; } +void RGBDFrame::saveCombinedImages(std::string path){ + unsigned int width = camera->width; + unsigned int height = camera->height; + cv::Mat combined; + combined.create(camera->height,2*camera->width,CV_8UC3); + unsigned char * combineddata = combined.data; + unsigned char * rgbdata = rgb.data; + unsigned short * depthdata = (unsigned short *)(depth.data); + + for(unsigned long h = 0; h < height;h++){ + for(unsigned long w = 0; w < width;w++){ + combineddata[3*(h*(2*width)+w)+0] = rgbdata[3*(h*width+w)+0]; + combineddata[3*(h*(2*width)+w)+1] = rgbdata[3*(h*width+w)+1]; + combineddata[3*(h*(2*width)+w)+2] = rgbdata[3*(h*width+w)+2]; + combineddata[3*(h*(2*width)+w+width)+0] = depthdata[h*width+w]/256; + combineddata[3*(h*(2*width)+w+width)+1] = depthdata[h*width+w]/256; + combineddata[3*(h*(2*width)+w+width)+2] = depthdata[h*width+w]/256; + } + } + + cv::namedWindow( "combined" , cv::WINDOW_AUTOSIZE ); + cv::imshow( "combined", combined ); + cv::waitKey(0); +} +void RGBDFrame::saveCombinedProcessedImages(std::string path){ + unsigned int width = camera->width; + unsigned int height = camera->height; + cv::Mat combined; + combined.create(camera->height,3*camera->width,CV_8UC3); + unsigned char * combineddata = combined.data; + float * normalsdata = (float *)(normals.data); + float * cedata = (float *)(ce.data); + float * dedata = (float *)(de.data); + + for(unsigned long h = 0; h < height;h++){ + for(unsigned long w = 0; w < width;w++){ + if(normalsdata[3*(h*width+w)+0] > 1){ + combineddata[3*(h*(3*width)+w+0*width)+0] = 255.0*normalsdata[3*(h*width+w)+0]; + combineddata[3*(h*(3*width)+w+0*width)+1] = 255.0*normalsdata[3*(h*width+w)+1]; + combineddata[3*(h*(3*width)+w+0*width)+2] = 255.0*normalsdata[3*(h*width+w)+2]; + }else{ + combineddata[3*(h*(3*width)+w+0*width)+0] = 0; + combineddata[3*(h*(3*width)+w+0*width)+1] = 0; + combineddata[3*(h*(3*width)+w+0*width)+2] = 0; + } + combineddata[3*(h*(3*width)+w+1*width)+0] = 255.0*cedata[3*(h*width+w)+0]; + combineddata[3*(h*(3*width)+w+1*width)+1] = 255.0*cedata[3*(h*width+w)+1]; + combineddata[3*(h*(3*width)+w+1*width)+2] = 255.0*cedata[3*(h*width+w)+2]; + combineddata[3*(h*(3*width)+w+2*width)+0] = 255.0*dedata[3*(h*width+w)+0]; + combineddata[3*(h*(3*width)+w+2*width)+1] = 255.0*dedata[3*(h*width+w)+1]; + combineddata[3*(h*(3*width)+w+2*width)+2] = 255.0*dedata[3*(h*width+w)+2]; + } + } + + cv::namedWindow( "combined" , cv::WINDOW_AUTOSIZE ); + cv::imshow( "combined", combined ); + cv::waitKey(0); +} + } From addb5482cca6f2139ba603eda69fa50442e48208 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 31 Oct 2016 22:50:45 +0100 Subject: [PATCH 175/255] added lots of functionality to store debugg visualization --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 7 +- quasimodo/quasimodo_brain/src/Util/Util.h | 2 +- .../metaroom_additional_view_processing.cpp | 15 +- .../quasimodo_models/include/core/RGBDFrame.h | 2 + .../quasimodo_models/include/model/Model.h | 2 + .../include/modelupdater/ModelUpdater.h | 2 +- .../include/registration/MassRegistration.h | 6 + .../weightfunctions/DistanceWeightFunction2.h | 3 + .../quasimodo_models/src/core/RGBDFrame.cpp | 605 ++++++------------ .../quasimodo_models/src/model/Model.cpp | 3 + .../src/modelupdater/ModelUpdater.cpp | 233 +++++-- .../src/registration/MassRegistration.cpp | 133 ++-- .../src/registration/MassRegistrationPPR2.cpp | 13 + .../DistanceWeightFunction2.cpp | 2 + .../DistanceWeightFunction2PPR.cpp | 3 + .../DistanceWeightFunction2PPR2.cpp | 3 + .../DistanceWeightFunction2PPR3.cpp | 67 +- 17 files changed, 577 insertions(+), 524 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 78bcbe6e..aafc4d5e 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -298,7 +298,7 @@ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath) return sweepmodel; } -void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, int debugg){ +void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, int debugg, std::string savePath){ double startTime = getTime(); printf("running segment method\n"); boost::shared_ptr viewer; @@ -309,6 +309,9 @@ void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > } reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); + if(savePath.size() != 0){ + massregmod->savePath = savePath+"/segment_"+std::to_string(models.front()->id); + } massregmod->timeout = 1200; massregmod->viewer = viewer; massregmod->visualizationLvl = debugg > 1; @@ -403,7 +406,7 @@ void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > std::vector movemask; std::vector dynmask; printf("computeMovingDynamicStatic\n"); - mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg);//Determine self occlusions + mu->computeMovingDynamicStatic(movemask,dynmask,bgcp,bgcf,model->relativeposes,model->frames,debugg,savePath);//Determine self occlusions external.push_back(movemask); internal.push_back(masks); dynamic.push_back(dynmask); diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index d64f580e..ebb4a7d0 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -55,7 +55,7 @@ std::vector getRegisteredViewPoses(const std::string& poses_fil Eigen::Matrix4d getMat(tf::StampedTransform tf); reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath = ""); -void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, int debugg = 0); +void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, int debugg = 0, std::string savePath = ""); std::vector loadModelsXML(std::string path); } diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 5e0b21b5..dda3be64 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -622,6 +622,7 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri sweep->recomputeModelPoints(); reglib::Model * fullmodel = new reglib::Model(); + fullmodel->savePath = savePath+"/"; fullmodel->frames = sweep->frames; fullmodel->relativeposes = sweep->relativeposes; fullmodel->modelmasks = sweep->modelmasks; @@ -658,6 +659,10 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri delete cd2; reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.25); + if(savePath.size() != 0){ + bgmassreg->savePath = savePath+"/processAV_"+std::to_string(fullmodel->id); + } + bgmassreg->timeout = 300; bgmassreg->viewer = viewer; bgmassreg->use_surface = true; @@ -814,6 +819,7 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: if(viewgroup_nrviews == (additional_nrviews+metaroom_nrviews) && !recomputeRelativePoses){ printf("time to read old\n"); fullmodel = new reglib::Model(); + fullmodel->savePath = saveVisuals_sp+"/"; for(unsigned int i = 0; i < viewgroup_nrviews; i++){ cv::Mat fullmask; @@ -830,13 +836,6 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); } - if(saveVisuals.size() > 0){ - for(unsigned int i = 0; i < fullmodel->frames.size(); i++){ - - } - } - - return fullmodel; } @@ -943,7 +942,7 @@ printf("bgs.size() = %i\n",bgs.size()); auto sweep = SimpleXMLParser::loadRoomFromXML(path, std::vector{},false); printf("models.front()->frames.size() = %i\n",models.front()->frames.size()); - quasimodo_brain::segment(bgs,models,internal,external,dynamic,visualization_lvl > 0); + quasimodo_brain::segment(bgs,models,internal,external,dynamic,visualization_lvl > 0,saveVisuals); remove_old_seg(sweep_folder); diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 3d280f42..750afa4b 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -29,6 +29,8 @@ namespace reglib { class RGBDFrame{ public: + static int saveId; + std::string keyval; Camera * camera; unsigned long id; diff --git a/quasimodo/quasimodo_models/include/model/Model.h b/quasimodo/quasimodo_models/include/model/Model.h index 07430e96..bab18aaa 100644 --- a/quasimodo/quasimodo_models/include/model/Model.h +++ b/quasimodo/quasimodo_models/include/model/Model.h @@ -32,6 +32,8 @@ using namespace Eigen; int last_changed; + std::string savePath; + std::vector points; std::vector< std::vector > all_keypoints; diff --git a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h index 002cedde..3d0ec141 100644 --- a/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h +++ b/quasimodo/quasimodo_models/include/modelupdater/ModelUpdater.h @@ -116,7 +116,7 @@ class ModelUpdater{ virtual std::vector computeDynamicObject(reglib::Model * bg, Eigen::Matrix4d bgpose, vector cp, vector cf, vector masks); virtual vector computeDynamicObject(vector bgcp, vector bgcf, vector bgmm, vector cp, vector cf, vector mm, vector poses, vector frames, vector masks, bool debugg = false); - virtual void computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); + virtual void computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg, std::string savePath = ""); //virtual SegmentationResults computeMovingDynamicStatic(vector bgcp, vector bgcf, vector poses, vector frames, bool debugg); virtual OcclusionScore computeOcclusionScore(vector & spvec, Matrix4d cp, RGBDFrame* cf, ModelMask* cm, int step = 1, bool debugg = false); diff --git a/quasimodo/quasimodo_models/include/registration/MassRegistration.h b/quasimodo/quasimodo_models/include/registration/MassRegistration.h index 7b1ce234..00b01539 100644 --- a/quasimodo/quasimodo_models/include/registration/MassRegistration.h +++ b/quasimodo/quasimodo_models/include/registration/MassRegistration.h @@ -67,6 +67,9 @@ namespace reglib unsigned int visualizationLvl; + + std::string savePath; + std::vector frames; std::vector mmasks; @@ -90,6 +93,9 @@ namespace reglib virtual void show(std::vector Xv, bool save = false, std::string filename = "", bool stop = true); Eigen::MatrixXd getMat(int rows, int cols, double * datas); + + virtual void savePCD(std::vector Xv, std::string path); + }; } diff --git a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h index 61830ce9..38c70231 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/DistanceWeightFunction2.h @@ -41,6 +41,9 @@ class DistanceWeightFunction2 double convergence_threshold; bool debugg_print; + std::string savePath; + std::stringstream saveData; + DistanceWeightFunction2(); ~DistanceWeightFunction2(); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index c3d37806..9cb45e6c 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -45,6 +45,9 @@ namespace reglib { + +int RGBDFrame::saveId = 0; + unsigned long RGBDFrame_id_counter; RGBDFrame::RGBDFrame(){ id = RGBDFrame_id_counter++; @@ -293,327 +296,6 @@ RGBDFrame * RGBDFrame::clone(){ return new RGBDFrame(camera->clone(), rgb.clone(),depth.clone(),capturetime, pose, true); } -//std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int blursize = 5){ -// cv::Mat src = frame->rgb.clone(); -// unsigned short * depthdata = (unsigned short *)(frame->depth.data); -// const float idepth = frame->camera->idepth_scale; - -// cv::GaussianBlur( src, src, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); - -// unsigned char * srcdata = (unsigned char *)src.data; -// unsigned int width = src.cols; -// unsigned int height = src.rows; - -// std::vector dxc; -// dxc.resize(width*height); -// for(unsigned int i = 0; i < width*height;i++){dxc[i] = 0;} - -// std::vector dyc; -// dyc.resize(width*height); -// for(unsigned int i = 0; i < width*height;i++){dyc[i] = 0;} - -// std::vector src_dxdata; -// src_dxdata.resize(width*height); -// for(unsigned int i = 0; i < width*height;i++){src_dxdata[i] = 0;} - -// std::vector src_dydata; -// src_dydata.resize(width*height); -// for(unsigned int i = 0; i < width*height;i++){src_dydata[i] = 0;} - -// std::vector maxima_dxdata; -// maxima_dxdata.resize(width*height); -// for(unsigned int i = 0; i < width*height;i++){maxima_dxdata[i] = 0;} - -// std::vector maxima_dydata; -// maxima_dydata.resize(width*height); -// for(unsigned int i = 0; i < width*height;i++){maxima_dydata[i] = 0;} - - -// for(unsigned int w = 0; w < width;w++){ -// for(unsigned int h = 0; h < height;h++){ -// int ind = h*width+w; -// src_dxdata[ind] = 0; -// src_dydata[ind] = 0; -// } -// } - -// unsigned int chans = 3; -// for(unsigned int c = 0; c < chans;c++){ -// for(unsigned int w = 1; w < width;w++){ -// for(unsigned int h = 1; h < height;h++){ -// int ind = h*width+w; -// int dir = 1; -// src_dxdata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-1)+c]) / 255.0)/3.0; - -// dir = width; -// src_dydata[ind] += fabs(float(srcdata[chans*ind+c] - srcdata[chans*(ind-width)+c]) / 255.0)/3.0; -// } -// } - -// } - -// for(unsigned int w = 1; w < width-1;w++){ -// for(unsigned int h = 1; h < height-1;h++){ -// int ind = h*width+w; -// maxima_dxdata[ind] = (src_dxdata[ind] >= src_dxdata[ind-1] && src_dxdata[ind] > src_dxdata[ind+1]); -// maxima_dydata[ind] = (src_dydata[ind] >= src_dydata[ind-width] && src_dydata[ind] > src_dydata[ind+width]); -// } -// } - -// std::vector< std::vector > probs; -// for(unsigned int c = 0; c < chans;c++){ -// std::vector Xvec; -// int dir; -// for(unsigned int w = 1; w < width-1;w++){ -// for(unsigned int h = 1; h < height-1;h++){ -// int ind = h*width+w; -// dir = 1; -// Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - -// dir = width; -// Xvec.push_back(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); -// } -// } - -// Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); -// for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} - -// double stdval = 0; -// for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} -// stdval = sqrt(stdval/double(Xvec.size())); - -// DistanceWeightFunction2PPR2 * func = new DistanceWeightFunction2PPR2(); -// func->zeromean = true; -// func->maxp = 0.9999; -// func->startreg = 0.5; -// func->debugg_print = false; -// //func->bidir = true; -// func->maxd = 256.0; -// func->histogram_size = 256; -// func->fixed_histogram_size = true; -// func->startmaxd = func->maxd; -// func->starthistogram_size = func->histogram_size; -// func->blurval = 0.005; -// func->stdval2 = stdval; -// func->maxnoise = stdval; -// func->reset(); -// func->computeModel(X); - -// std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} -// std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} -// for(unsigned int w = 1; w < width;w++){ -// for(unsigned int h = 1; h < height-1;h++){ -// int ind = h*width+w; - -// dir = 1; -// dx[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); - -// dir = width; -// dy[ind] = func->getProb(fabs(srcdata[chans*ind+c] - srcdata[chans*(ind-dir)+c])); -// } -// } -// delete func; - -// probs.push_back(dx); -// probs.push_back(dy); -// } - -// { -// std::vector Xvec; -// for(unsigned int w = 1; w < width-1;w++){ -// for(unsigned int h = 1; h < height-1;h++){ -// int ind = h*width+w; -// float z = idepth*float(depthdata[ind]); - -// if(w > 1){ -// int dir = -1; -// int other2 = ind+2*dir; -// int other = ind+dir; - -// float z3 = idepth*float(depthdata[other2]); -// float z2 = idepth*float(depthdata[other]); -// z2 = 2*z2-z3; - -// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} -// } - -// if(h > 1){ -// int dir = -width; -// int other2 = ind+2*dir; -// int other = ind+dir; - -// float z3 = idepth*float(depthdata[other2]); -// float z2 = idepth*float(depthdata[other]); -// z2 = 2*z2-z3; - -// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} -// } -// } -// } - -// Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); -// for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} - -// double stdval = 0; -// for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} -// stdval = sqrt(stdval/double(Xvec.size())); - -// DistanceWeightFunction2PPR2 * funcZ = new DistanceWeightFunction2PPR2(); -// funcZ->zeromean = true; -// funcZ->startreg = 0.002; -// funcZ->debugg_print = false; -// //funcZ->bidir = true; -// funcZ->maxp = 0.999999; -// funcZ->maxd = 0.1; -// funcZ->histogram_size = 100; -// funcZ->fixed_histogram_size = true; -// funcZ->startmaxd = funcZ->maxd; -// funcZ->starthistogram_size = funcZ->histogram_size; -// funcZ->blurval = 0.5; -// funcZ->stdval2 = stdval; -// funcZ->maxnoise = stdval; -// funcZ->reset(); -// funcZ->computeModel(X); - -// for(unsigned int w = 1; w < width-1;w++){ -// for(unsigned int h = 1; h < height-1;h++){ -// int ind = h*width+w; -// float z = idepth*float(depthdata[ind]); - -// if(w > 1){ -// int dir = -1; -// int other2 = ind+2*dir; -// int other = ind+dir; - -// float z3 = idepth*float(depthdata[other2]); -// float z2 = idepth*float(depthdata[other]); -// z2 = 2*z2-z3; - -// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} -// } - -// if(h > 1){ -// int dir = -width; -// int other2 = ind+2*dir; -// int other = ind+dir; - -// float z3 = idepth*float(depthdata[other2]); -// float z2 = idepth*float(depthdata[other]); -// z2 = 2*z2-z3; - -// if(z2 > 0 || z > 0){Xvec.push_back((z-z2)/(z*z+z2*z2));} -// } -// } -// } - -// std::vector dx; dx.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dx[i] = 0.5;} -// std::vector dy; dy.resize(width*height); for(unsigned int i = 0; i < width*height;i++){dy[i] = 0.5;} - -// for(unsigned int w = 1; w < width-1;w++){ -// for(unsigned int h = 1; h < height-1;h++){ -// int ind = h*width+w; -// float z = idepth*float(depthdata[ind]); - -// if(w > 1){ -// int dir = -1; -// int other2 = ind+2*dir; -// int other = ind+dir; - - -// float z3 = idepth*float(depthdata[other2]); -// float z2 = idepth*float(depthdata[other]); - -// float dz = fabs(z-z2); -// if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} -// if(z2 > 0 || z > 0){dx[ind] = funcZ->getProb(dz/(z*z+z2*z2));} -// } - -// if(h > 1){ -// int dir = -width; - -// int other2 = ind+2*dir; -// int other = ind+dir; - -// float z3 = idepth*float(depthdata[other2]); -// float z2 = idepth*float(depthdata[other]); - -// float dz = fabs(z-z2); -// if(z3 > 0){dz = std::min(dz,fabs(z- (2*z2-z3)));} -// if(z2 > 0 || z > 0){dy[ind] = funcZ->getProb(dz/(z*z+z2*z2));} -// } -// } -// } - -// delete funcZ; -// probs.push_back(dx); -// probs.push_back(dy); -// } - - -// for(unsigned int w = 0; w < width;w++){ -// for(unsigned int h = 0; h < height;h++){ -// int ind = h*width+w; - -// float probX = 0; -// float probY = 0; - -// if(w > 0 && w < width-1){ -// float ax = 0.5; -// float bx = 0.5; -// for(unsigned int p = 0; p < probs.size()-2; p+=2){ -// float pr = probs[p][ind]; -// ax *= pr; -// bx *= 1.0-pr; -// } -// float px = ax/(ax+bx); -// float current = 0; -// if(!frame->det_dilate.data[ind]){ current = (1-px) * float(maxima_dxdata[ind]);} -// else{ current = std::max(float(1-probs[probs.size()-2][ind]),0.8f*(1-px) * float(maxima_dxdata[ind]));} -// probX = 1-current; -// } - -// if(h > 0 && h < height-1){ -// float ay = 0.5; -// float by = 0.5; -// for(unsigned int p = 1; p < probs.size()-2; p+=2){ -// float pr = probs[p][ind]; -// ay *= pr; -// by *= 1.0-pr; -// } -// float py = ay/(ay+by); -// float current = 0; -// if(!frame->det_dilate.data[ind]){ current = (1-py) * float(maxima_dydata[ind]);} -// else{ current = std::max(float(1-probs[probs.size()-1][ind]),0.8f*(1-py) * float(maxima_dydata[ind]));} -// probY = 1-current; -// } - -// dxc[ind] = std::min(probX,probY); -// dyc[ind] = std::min(probX,probY); -// } -// } - - -// std::vector< std::vector > probs2; -// probs2.push_back(dxc); -// probs2.push_back(dyc); - -// cv::Mat edges; -// edges.create(height,width,CV_32FC3); -// float * edgesdata = (float *)edges.data; - -// for(unsigned int i = 0; i < width*height;i++){ -// edgesdata[3*i+0] = 0; -// edgesdata[3*i+1] = dxc[i]; -// edgesdata[3*i+2] = dyc[i]; -// } - -// // cv::namedWindow( "src", cv::WINDOW_AUTOSIZE ); cv::imshow( "src", src); -// // cv::namedWindow( "edges", cv::WINDOW_AUTOSIZE ); cv::imshow( "edges", edges); -// // cv::waitKey(0); - -// return probs2; -//} - RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals, std::string savePath){ printf("savepath: %s\n",savePath.c_str()); bool verbose = false; @@ -631,9 +313,14 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt capturetime = capturetime_; pose = pose_; - if(savePath.size() != 0){ - cv::imwrite( savePath+"/rgb.png", rgb ); - cv::imwrite( savePath+"/depth.png", depth ); + char savestr [1024]; + sprintf(savestr,"%s/frame%5.5i_",savePath.c_str(),saveId); + if(savePath.size() != 0){saveId++;} + + + if(savePath.size() != 0){ + cv::imwrite( std::string(savestr)+"rgb.png", rgb ); + cv::imwrite( std::string(savestr)+"depth.png", depth ); } IplImage iplimg = rgb_; @@ -698,24 +385,6 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float pred3 = std::min(meanpred,fabs(z1-z2)); dedata[3*ind+1] = pred3/noise3; Xvec.push_back(meanpred/noise3); - -// if (myset.count(ind)!=0){ -// printf("%i :: ",ind); -// pn(z0); -// printf(" "); -// pn(z1); -// printf(" "); -// pn(z2); -// printf(" "); -// pn(z3); -// printf("-->"); -// pn(pred1); -// printf(" "); -// pn(pred2); -// printf("-->"); -// pn(Xvec.back()); -// printf("\n"); -// } } } @@ -741,38 +410,11 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt Xvec.push_back(meanpred/noise3); } } - -/* - if(w > 1){ - float z2 = 0.5*idepth*float(depthdata[ind-step]+depthdata[ind+step]); - double noise1 = z*z; - double noise2 = z2*z2; - double noise3 = sqrt(noise1*noise1+noise2*noise2); - if(z2 > 0 || z > 0){ - //131 , 376 :: 240771 -> 240770 - if(ind == 240771){ - printf("ind: %i edge: %f before: %f current: %f after: %f",ind,fabs((z-z2)/noise3),idepth*float(depthdata[ind-step]),idepth*float(depthdata[ind]),idepth*float(depthdata[ind+step])); - } - dedata[3*ind+1] = fabs((z-z2)/noise3); - Xvec.push_back(dedata[3*ind+1]); - } - } - - if(h > 1){ - float z2 = 0.5*idepth*float(depthdata[ind-step*width]+depthdata[ind+step*width]); - double noise1 = z*z; - double noise2 = z2*z2; - double noise3 = sqrt(noise1*noise1+noise2*noise2); - if(z2 > 0 || z > 0){ - dedata[3*ind+2] = fabs((z-z2)/noise3); - Xvec.push_back(dedata[3*ind+2]); - } - } -*/ - } } + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"depth_diff.png", 4.0*255.0*de );} + if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); @@ -784,6 +426,9 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt stdval = sqrt(stdval/double(Xvec.size())); DistanceWeightFunction2PPR3 * funcZ = new DistanceWeightFunction2PPR3(); + if(savePath.size() != 0){ + funcZ->savePath = std::string(savestr)+"funcZ.m"; + } funcZ->zeromean = true; //funcZ->startreg = 0.001; funcZ->startreg = 0.0001; @@ -809,6 +454,8 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"depth_prob.png", 255.0*de );} + delete funcZ; if(verbose) @@ -817,6 +464,9 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt int blursize = 5; cv::Mat blur_rgb = rgb.clone(); cv::GaussianBlur( blur_rgb, blur_rgb, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); + + + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"blur_rgb.png", blur_rgb );} unsigned char * blurdata = (unsigned char *)blur_rgb.data; cv::Mat re; @@ -841,7 +491,97 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt std::vector XvecR; std::vector XvecG; std::vector XvecB; - for(unsigned int w = 1; w < width;w++){ + +// for(unsigned int w = step; w < width-step;w++){ +// for(unsigned int h = step; h < height-step;h++){ +// int ind = h*width+w; +// float b = float(blurdata[3*ind+0]); +// float g = float(blurdata[3*ind+1]); +// float r = float(blurdata[3*ind+2]); + +// if(w > 2 && w < width-1){ +// float b0 = float(blurdata[3*(ind-2)+0]); +// float b1 = float(blurdata[3*(ind-1)+0]); +// float b2 = b; +// float b3 = float(blurdata[3*(ind+1)+0]); + + +// float bpred1 = b1 + (b1-b0); +// float bpred2 = b2 - (b3-b2); + +// //printf("%3.3f %3.3f %3.3f %3.3f -> %3.3f %3.3f -> %3.3f %3.3f -> %3.3f\n",b0,b1,b2,b3,bpred1,bpred2,fabs(b2-bpred1),fabs(b1-bpred2),fabs(b2-bpred1) + fabs(b1-bpred2)); + +// bedata[3*ind+1] = 0.5*(fabs(b2-bpred1) + fabs(b1-bpred2)); +// XvecB.push_back(bedata[3*ind+1]); + + +// float g0 = float(blurdata[3*(ind-2)+1]); +// float g1 = float(blurdata[3*(ind-1)+1]); +// float g2 = g; +// float g3 = float(blurdata[3*(ind+1)+1]); + +// float gpred1 = g1 + (g1-g0); +// float gpred2 = g2 - (g3-g2); + +// gedata[3*ind+1] = 0.5*(fabs(g2-gpred1) + fabs(g1-gpred2)); +// XvecG.push_back(gedata[3*ind+1]); + + +// float r0 = float(blurdata[3*(ind-2)+2]); +// float r1 = float(blurdata[3*(ind-1)+2]); +// float r2 = r; +// float r3 = float(blurdata[3*(ind+1)+2]); + +// float rpred1 = r1 + (r1-r0); +// float rpred2 = r2 - (r3-r2); + +// redata[3*ind+1] = 0.5*(fabs(r2-rpred1) + fabs(r1-rpred2)); +// XvecR.push_back(redata[3*ind+1]); + + +// } + +// if(h > 2 && h < height-1){ +// float b0 = float(blurdata[3*(ind-2*width)+0]); +// float b1 = float(blurdata[3*(ind-1*width)+0]); +// float b2 = b; +// float b3 = float(blurdata[3*(ind+1*width)+0]); + +// float bpred1 = b1 + (b1-b0); +// float bpred2 = b2 - (b3-b2); + +// bedata[3*ind+2] = 0.5*(fabs(b2-bpred1) + fabs(b1-bpred2)); +// XvecB.push_back(bedata[3*ind+2]); + + +// float g0 = float(blurdata[3*(ind-2*width)+1]); +// float g1 = float(blurdata[3*(ind-1*width)+1]); +// float g2 = g; +// float g3 = float(blurdata[3*(ind+1*width)+1]); + +// float gpred1 = g1 + (g1-g0); +// float gpred2 = g2 - (g3-g2); + +// gedata[3*ind+2] = 0.5*(fabs(g2-gpred1) + fabs(g1-gpred2)); +// XvecG.push_back(gedata[3*ind+2]); + + +// float r0 = float(blurdata[3*(ind-2*width)+2]); +// float r1 = float(blurdata[3*(ind-1*width)+2]); +// float r2 = r; +// float r3 = float(blurdata[3*(ind+1*width)+2]); + +// float rpred1 = r1 + (r1-r0); +// float rpred2 = r2 - (r3-r2); + +// redata[3*ind+2] = 0.5*(fabs(r2-rpred1) + fabs(r1-rpred2)); +// XvecR.push_back(redata[3*ind+2]); + +// } +// } +// } + + for(unsigned int w = 1; w < width-1;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; bedata[3*ind+1] = fabs(blurdata[3*ind+0] - blurdata[3*(ind-1)+0]); @@ -855,7 +595,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 1; h < height;h++){ + for(unsigned int h = 1; h < height-1;h++){ int ind = h*width+w; bedata[3*ind+2] = fabs(blurdata[3*ind+0] - blurdata[3*(ind-width)+0]); XvecB.push_back(bedata[3*ind+1]); @@ -868,6 +608,38 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } +// for(unsigned int w = 1; w < width-1;w++){ +// for(unsigned int h = 0; h < height;h++){ +// int ind = h*width+w; +// bedata[3*ind+1] = fabs(blurdata[3*(ind+0)+0]-0.5*(blurdata[3*(ind+1)+0] + blurdata[3*(ind-1)+0])); +// XvecB.push_back(bedata[3*ind+1]); + +// gedata[3*ind+1] = fabs(blurdata[3*(ind+0)+1]-0.5*(blurdata[3*(ind+1)+1] + blurdata[3*(ind-1)+1])); +// XvecG.push_back(gedata[3*ind+1]); + +// redata[3*ind+1] = fabs(blurdata[3*(ind+0)+2]-0.5*(blurdata[3*(ind+1)+2] + blurdata[3*(ind-1)+2])); +// XvecR.push_back(redata[3*ind+1]); +// } +// } +// for(unsigned int w = 0; w < width;w++){ +// for(unsigned int h = 1; h < height-1;h++){ +// int ind = h*width+w; +// bedata[3*ind+2] = fabs(blurdata[3*(ind+0)+0]-0.5*(blurdata[3*(ind+width)+0] + blurdata[3*(ind-width)+0])); +// XvecB.push_back(bedata[3*ind+1]); + +// gedata[3*ind+2] = fabs(blurdata[3*(ind+0)+1]-0.5*(blurdata[3*(ind+width)+1] + blurdata[3*(ind-width)+1])); +// XvecG.push_back(gedata[3*ind+1]); + +// redata[3*ind+2] = fabs(blurdata[3*(ind+0)+2]-0.5*(blurdata[3*(ind+width)+2] + blurdata[3*(ind-width)+2])); +// XvecR.push_back(redata[3*ind+1]); +// } +// } + + + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_diff.png", 3.0*re );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_diff.png", 3.0*ge );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"be_diff.png", 3.0*be );} + std::vector src_dxdata; src_dxdata.resize(nr_pixels); for(unsigned int i = 0; i < nr_pixels;i++){src_dxdata[i] = 0;} @@ -926,12 +698,29 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); - DistanceWeightFunction2PPR2 * funcR = new DistanceWeightFunction2PPR2(); - DistanceWeightFunction2PPR2 * funcG = new DistanceWeightFunction2PPR2(); - DistanceWeightFunction2PPR2 * funcB = new DistanceWeightFunction2PPR2(); + GeneralizedGaussianDistribution * ggdfuncR = new GeneralizedGaussianDistribution(true,false); + ggdfuncR->nr_refineiters = 1; + GeneralizedGaussianDistribution * ggdfuncG = new GeneralizedGaussianDistribution(true,false); + ggdfuncG->nr_refineiters = 1; + GeneralizedGaussianDistribution * ggdfuncB = new GeneralizedGaussianDistribution(true,false); + ggdfuncB->nr_refineiters = 1; + + DistanceWeightFunction2PPR3 * funcR = new DistanceWeightFunction2PPR3(ggdfuncR); + DistanceWeightFunction2PPR3 * funcG = new DistanceWeightFunction2PPR3(ggdfuncG); + DistanceWeightFunction2PPR3 * funcB = new DistanceWeightFunction2PPR3(ggdfuncB); + + + + if(savePath.size() != 0){ + funcR->savePath = std::string(savestr)+"funcR.m"; + funcG->savePath = std::string(savestr)+"funcG.m"; + funcB->savePath = std::string(savestr)+"funcB.m"; + } + + funcR->zeromean = funcG->zeromean = funcB->zeromean = true; funcR->maxp = funcG->maxp = funcB->maxp = 0.999999999999999999; - funcR->startreg = funcG->startreg = funcB->startreg = 0.1; + funcR->startreg = funcG->startreg = funcB->startreg = 0.001; funcR->debugg_print = funcG->debugg_print = funcB->debugg_print = false; funcR->maxd = funcG->maxd = funcB->maxd = 256.0; funcR->histogram_size = funcG->histogram_size = funcB->histogram_size = 256; @@ -955,8 +744,6 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt funcB->reset(); ((DistanceWeightFunction2*)funcB)->computeModel(XvecB); - - for(unsigned int w = 1; w < width-1;w++){ for(unsigned int h = 1; h < height-1;h++){ int ind = h*width+w; @@ -988,6 +775,12 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt delete funcG; delete funcB; + + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_prob.png", 127.0*re );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_prob.png", 127.0*ge );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"be_prob.png", 127.0*be );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ce_prob.png", 127.0*ce );} + if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); @@ -1007,11 +800,11 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt cenmsdata[3*i+0] = edgep; cenmsdata[3*i+1] = edgep; cenmsdata[3*i+2] = edgep; -// cenmsdata[3*i+0] = 0; -// cenmsdata[3*i+1] = std::max(0.0001f,cedata[3*i+1]*maxima_dxdata[i]); -// cenmsdata[3*i+2] = std::max(0.0001f,cedata[3*i+2]*maxima_dydata[i]); } + + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ce_non_maxima_supress_prob.png", 255.0*cenms );} + // for(int i = 0; i < nr_pixels; i++){ // double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]); // edgep = std::max(0.00001,edgep); @@ -1099,14 +892,40 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } } + + if(savePath.size() != 0){ + cv::Mat out; + out.create(height,width,CV_8UC3); + unsigned char * outdata = out.data; + + for(int w = 0; w < width; w++){ + for(int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::Normal p2 = normals_cloud->points[ind]; + if(!isnan(p2.normal_x)){ + outdata[3*ind+0] = fabs(255.0*p2.normal_x); + outdata[3*ind+1] = fabs(255.0*p2.normal_y); + outdata[3*ind+2] = fabs(255.0*p2.normal_z); + }else{ + outdata[3*ind+0] = 0; + outdata[3*ind+1] = 0; + outdata[3*ind+2] = 0; + } + } + } + + cv::imwrite( std::string(savestr)+"normals.png", out ); + } } if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); printf("complete time to create RGBD image: %5.5fs\n",getTime()-completeStartTime); //getImageProbs(); - saveCombinedImages(""); - saveCombinedProcessedImages(""); + if(savePath.size() != 0){ + saveCombinedImages(std::string(savestr)+"Combined.png"); + saveCombinedProcessedImages(std::string(savestr)+"CombinedProcessed.png"); + } } RGBDFrame::~RGBDFrame(){ @@ -1540,9 +1359,7 @@ void RGBDFrame::saveCombinedImages(std::string path){ } } - cv::namedWindow( "combined" , cv::WINDOW_AUTOSIZE ); - cv::imshow( "combined", combined ); - cv::waitKey(0); + cv::imwrite( path, combined ); } void RGBDFrame::saveCombinedProcessedImages(std::string path){ unsigned int width = camera->width; @@ -1557,9 +1374,9 @@ void RGBDFrame::saveCombinedProcessedImages(std::string path){ for(unsigned long h = 0; h < height;h++){ for(unsigned long w = 0; w < width;w++){ if(normalsdata[3*(h*width+w)+0] > 1){ - combineddata[3*(h*(3*width)+w+0*width)+0] = 255.0*normalsdata[3*(h*width+w)+0]; - combineddata[3*(h*(3*width)+w+0*width)+1] = 255.0*normalsdata[3*(h*width+w)+1]; - combineddata[3*(h*(3*width)+w+0*width)+2] = 255.0*normalsdata[3*(h*width+w)+2]; + combineddata[3*(h*(3*width)+w+0*width)+0] = 255.0*fabs(normalsdata[3*(h*width+w)+0]); + combineddata[3*(h*(3*width)+w+0*width)+1] = 255.0*fabs(normalsdata[3*(h*width+w)+1]); + combineddata[3*(h*(3*width)+w+0*width)+2] = 255.0*fabs(normalsdata[3*(h*width+w)+2]); }else{ combineddata[3*(h*(3*width)+w+0*width)+0] = 0; combineddata[3*(h*(3*width)+w+0*width)+1] = 0; @@ -1574,9 +1391,11 @@ void RGBDFrame::saveCombinedProcessedImages(std::string path){ } } - cv::namedWindow( "combined" , cv::WINDOW_AUTOSIZE ); - cv::imshow( "combined", combined ); - cv::waitKey(0); + cv::imwrite( path, combined ); + +// cv::namedWindow( "combined" , cv::WINDOW_AUTOSIZE ); +// cv::imshow( "combined", combined ); +// cv::waitKey(0); } } diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 70ecfd6c..7e407d09 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -13,6 +13,7 @@ Model::Model(){ score = 0; id = model_id_counter++; last_changed = -1; + savePath = ""; } Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){ @@ -29,6 +30,8 @@ Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){ relativeposes.push_back(pose); frames.push_back(frame); modelmasks.push_back(new ModelMask(mask)); + + savePath = ""; //recomputeModelPoints(); } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index d86ba52a..94d2b28c 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2372,7 +2372,10 @@ int setupPriors(int method,float current_occlusions, float current_overlaps, flo return 0; } -void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg){ +void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, std::vector & dynmask, vector bgcp, vector bgcf, vector poses, vector frames, bool debugg, std::string savePath){ + static int segment_run_counter = -1; + segment_run_counter++; + double computeMovingDynamicStatic_startTime = getTime(); SegmentationResults sr; @@ -2443,6 +2446,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s DistanceWeightFunction2PPR3 * dfuncTMP = new DistanceWeightFunction2PPR3(dggdnfunc); dfunc = dfuncTMP; dfuncTMP->startreg = 0.00; + dfuncTMP->max_under_mean = false; dfuncTMP->debugg_print = false; dfuncTMP->bidir = true; dfuncTMP->zeromean = false; @@ -2457,6 +2461,11 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dfuncTMP->compute_infront = true; dfuncTMP->ggd = true; dfuncTMP->reset(); + + if(savePath.size() != 0){ + dfuncTMP->savePath = std::string(savePath)+"/segment_"+std::to_string(segment_run_counter)+"_dfunc.m"; + } + dfunc->computeModel(dvec); GeneralizedGaussianDistribution * ggdnfunc = new GeneralizedGaussianDistribution(true,true); @@ -2480,6 +2489,12 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s nfuncTMP->reset(); nfunc->computeModel(nvec); + if(savePath.size() != 0){ + nfuncTMP->savePath = std::string(savePath)+"/segment_"+std::to_string(segment_run_counter)+"_nfunc.m"; + } + + + printf("training time: %5.5fs\n",getTime()-startTime); long frameConnections = 0; @@ -2556,9 +2571,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s total_Dynw += getTime()-startTime; - cv::Mat priorsimg; - priorsimg.create(height,width,CV_32FC3); - float * priorsdata = (float*)priorsimg.data; + startTime = getTime(); unsigned char * detdata = (unsigned char*)(frame->det_dilate.data); @@ -2572,10 +2585,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s priors[3*(offset+ind)+0], priors[3*(offset+ind)+1],priors[3*(offset+ind)+2], prior_weights[2*(offset+ind)+0], prior_weights[2*(offset+ind)+1]); - priorsdata[3*ind+0] = priors[3*(offset+ind)+0]; - priorsdata[3*ind+1] = priors[3*(offset+ind)+1]; - priorsdata[3*ind+2] = priors[3*(offset+ind)+2]; - if(probs[0][ind] > 0.00000001){frameConnections++;} if(probs[1][ind] > 0.00000001){frameConnections++;} @@ -2618,25 +2627,8 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s if(debugg != 0){printf("local inference time: %10.10fs\n\n",getTime()-start_inf);} - // cv::Mat edgeimg; - // edgeimg.create(height,width,CV_32FC3); - // float * edgedata = (float*)edgeimg.data; - // for(int j = 0; j < width*height; j++){ - // edgedata[3*j+0] = 0; - // edgedata[3*j+1] = 1-probs[0][j]; - // edgedata[3*j+2] = 1-probs[1][j]; - // } - // cv::Mat labelimg; - // labelimg.create(height,width,CV_32FC3); - // float * labelimgdata = (float*)labelimg.data; - // for(unsigned long ind = 0; ind < nr_pixels;ind++){ - // double label = g->what_segment(ind); - // labelimgdata[3*ind+0] = 0; - // labelimgdata[3*ind+1] = label; - // labelimgdata[3*ind+2] = 1-label; - // } // cv::Mat detrgb; // detrgb.create(height,width,CV_8UC3); @@ -2658,6 +2650,84 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s // cv::waitKey(0); + if(savePath.size() != 0){ + cv::Mat edgeimg; + edgeimg.create(height,width,CV_8UC3); + unsigned char * edgedata = (unsigned char*)edgeimg.data; + + for(int j = 0; j < width*height; j++){ + edgedata[3*j+0] = 0; + edgedata[3*j+1] = 255.0*(1-probs[0][j]); + edgedata[3*j+2] = 255.0*(1-probs[1][j]); + } + + cv::Mat labelimg; + labelimg.create(height,width,CV_8UC3); + unsigned char * labelimgdata = (unsigned char*)labelimg.data; + for(unsigned long ind = 0; ind < nr_pixels;ind++){ + double label = g->what_segment(ind); + labelimgdata[3*ind+0] = 255*label; + labelimgdata[3*ind+1] = 255*label; + labelimgdata[3*ind+2] = 255*label; + } + + cv::Mat priorsimg; + priorsimg.create(height,width,CV_8UC3); + unsigned char * priorsdata = (unsigned char*)priorsimg.data; + for(unsigned long ind = 0; ind < nr_pixels;ind++){ + priorsdata[3*ind+0] = 255.0*priors[3*(offset+ind)+2]; + priorsdata[3*ind+1] = 255.0*priors[3*(offset+ind)+1]; + priorsdata[3*ind+2] = 255.0*priors[3*(offset+ind)+0]; + } + + cv::Mat current_overlapsimg; + current_overlapsimg.create(height,width,CV_8UC3); + unsigned char * current_overlapsdata = (unsigned char*)current_overlapsimg.data; + for(unsigned long ind = 0; ind < nr_pixels;ind++){ + current_overlapsdata[3*ind+0] = 255.0*current_overlaps[ind]; + current_overlapsdata[3*ind+1] = 255.0*current_overlaps[ind]; + current_overlapsdata[3*ind+2] = 255.0*current_overlaps[ind]; + } + + cv::Mat bg_overlapsimg; + bg_overlapsimg.create(height,width,CV_8UC3); + unsigned char * bg_overlapsdata = (unsigned char*)bg_overlapsimg.data; + for(unsigned long ind = 0; ind < nr_pixels;ind++){ + bg_overlapsdata[3*ind+0] = 255.0*bg_overlaps[ind]; + bg_overlapsdata[3*ind+1] = 255.0*bg_overlaps[ind]; + bg_overlapsdata[3*ind+2] = 255.0*bg_overlaps[ind]; + } + + cv::Mat current_occlusionsimg; + current_occlusionsimg.create(height,width,CV_8UC3); + unsigned char * current_occlusionsdata = (unsigned char*)current_occlusionsimg.data; + for(unsigned long ind = 0; ind < nr_pixels;ind++){ + current_occlusionsdata[3*ind+0] = 255.0*current_occlusions[ind]; + current_occlusionsdata[3*ind+1] = 255.0*current_occlusions[ind]; + current_occlusionsdata[3*ind+2] = 255.0*current_occlusions[ind]; + } + + cv::Mat bg_occlusionsimg; + bg_occlusionsimg.create(height,width,CV_8UC3); + unsigned char * bg_occlusionsdata = (unsigned char*)bg_occlusionsimg.data; + for(unsigned long ind = 0; ind < nr_pixels;ind++){ + bg_occlusionsdata[3*ind+0] = 255.0*bg_occlusions[ind]; + bg_occlusionsdata[3*ind+1] = 255.0*bg_occlusions[ind]; + bg_occlusionsdata[3*ind+2] = 255.0*bg_occlusions[ind]; + } + + cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_current_overlapsimg.png", current_overlapsimg ); + cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_bg_overlapsimg.png", bg_overlapsimg ); + cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_current_occlusionsimg.png", current_occlusionsimg ); + cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_bg_occlusionsimg.png", bg_occlusionsimg ); + cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_edgeimg.png", edgeimg ); + cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_rgb.png", frame->rgb ); + cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_priors.png", priorsimg ); + cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_labelimg.png", labelimg ); + } + + + delete g; @@ -2802,7 +2872,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s for(unsigned int j = 0; j < interframe_connectionId[i].size();j++){ double weight = interframe_connectionStrength[i][j]; unsigned long other = interframe_connectionId[i][j]; - if(weight > 0.01 && labels[i] != labels[other]){//g->what_segment(i) != g->what_segment(other)){ + if(weight > 0.01 && labels[i] != labels[other]){ if(rand() <= prob*RAND_MAX){ trueadds++; g -> add_edge( i, other, weight, weight ); @@ -2999,44 +3069,83 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } movemask.push_back(m); dynmask.push_back(d); + } + + if(savePath.size() != 0){ + pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); + cloud_sample->points.resize(current_point); + cloud_sample->width = current_point; + cloud_sample->height = 1; + for(unsigned int i = 0; i < current_point; i++){ + cloud_sample->points[i].r = priors[3*i+0]*255.0; + cloud_sample->points[i].g = priors[3*i+1]*255.0; + cloud_sample->points[i].b = priors[3*i+2]*255.0; + } + pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_priors.pcd", *cloud_sample); - // unsigned short * depthdata = (unsigned short *)(frames[i]->depth.data); - // cv::Mat debuggimg; - // debuggimg.create(height,width,CV_8UC3); - // unsigned char * debuggimgdata = (unsigned char*)debuggimg.data; - // cv::Mat edgeimg; - // edgeimg.create(height,width,CV_32FC3); - // float * edgedata = (float*)edgeimg.data; - // for(unsigned int label = 0; label < labelID.size(); label++){ - // int lid = labelID[label]; - // printf("lid: %i\n",lid); - // if(lid <= 0){continue;} - // bool hasdata = false; - // for(int j = 0; j < width*height; j++){ - // if(ddata[j] == lid){ - // debuggimgdata[3*j+0] = 255; - // debuggimgdata[3*j+1] = 255; - // debuggimgdata[3*j+2] = 255; - // hasdata = true; - // }else{ - // debuggimgdata[3*j+0] = 0; - // debuggimgdata[3*j+1] = 0; - // debuggimgdata[3*j+2] = 0; - // } - // if(depthdata[j] == 0){ - // debuggimgdata[3*j+0] = 255; - // debuggimgdata[3*j+1] = 0; - // debuggimgdata[3*j+2] = 255; - // } - // } - // if(hasdata){ - // cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", frames[i]->rgb ); - // cv::namedWindow( "debuggimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "debuggimg", debuggimg ); - // cv::namedWindow( "edgeimg" , cv::WINDOW_AUTOSIZE ); cv::imshow( "edgeimg", edgeimg ); - // cv::waitKey(0); - // } - // } + for(unsigned int i = 0; i < current_point; i++){ + cloud_sample->points[i].r = 0; + cloud_sample->points[i].g = 0; + cloud_sample->points[i].b = 255; + } + + for(unsigned int c = 0; c < sr.component_dynamic.size(); c++){ + for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){ + cloud_sample->points[sr.component_dynamic[c][i]].r = 0; + cloud_sample->points[sr.component_dynamic[c][i]].g = 255; + cloud_sample->points[sr.component_dynamic[c][i]].b = 0; + } + } + + for(unsigned int c = 0; c < sr.component_moving.size(); c++){ + for(unsigned int i = 0; i < sr.component_moving[c].size(); i++){ + cloud_sample->points[sr.component_moving[c][i]].r = 255; + cloud_sample->points[sr.component_moving[c][i]].g = 0; + cloud_sample->points[sr.component_moving[c][i]].b = 0; + } + } + pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_classes.pcd", *cloud_sample); + + for(unsigned int i = 0; i < current_point; i++){ + cloud_sample->points[i].r = 0; + cloud_sample->points[i].g = 0; + cloud_sample->points[i].b = 255; + } + + for(unsigned int c = 0; c < sr.component_dynamic.size(); c++){ + int randr = rand()%256; + int randg = rand()%256; + int randb = rand()%256; + if(randr*randr + randg*randg + (255-randr)*(255-randr) < 1000){ + randr = rand()%256; + randg = rand()%256; + randb = rand()%256; + } + for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){ + cloud_sample->points[sr.component_dynamic[c][i]].r = randr; + cloud_sample->points[sr.component_dynamic[c][i]].g = randg; + cloud_sample->points[sr.component_dynamic[c][i]].b = randb; + } + } + + for(unsigned int c = 0; c < sr.component_moving.size(); c++){ + int randr = rand()%256; + int randg = rand()%256; + int randb = rand()%256; + if(randr*randr + randg*randg + (255-randr)*(255-randr) < 1000){ + randr = rand()%256; + randg = rand()%256; + randb = rand()%256; + } + for(unsigned int i = 0; i < sr.component_moving[c].size(); i++){ + cloud_sample->points[sr.component_moving[c][i]].r = randr; + cloud_sample->points[sr.component_moving[c][i]].g = randg; + cloud_sample->points[sr.component_moving[c][i]].b = randb; + } + } + pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_clusters.pcd", *cloud_sample); + } if(debugg){ diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp index 54bc38a7..68fa68f4 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistration.cpp @@ -17,7 +17,7 @@ MassRegistration::MassRegistration(){ nomask = true; maskstep = 1; nomaskstep = 100000; - timeout = 60;//1 minute timeout + timeout = 60;//1 minute timeout } MassRegistration::~MassRegistration(){} @@ -82,66 +82,87 @@ Eigen::MatrixXd MassRegistration::getMat(int rows, int cols, double * datas){ void MassRegistration::show(std::vector Xv, bool save, std::string filename, bool stop){ - printf("show\n"); + printf("show\n"); //for(unsigned int a = 0; a < Xv.size(); a++){ - viewer->removeAllPointClouds(); - - srand(0); - for(unsigned int xi = 0; xi < Xv.size(); xi++){ - - Eigen::MatrixXd X = Xv[xi]; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - int r,g,b; -// if(xi == a){ -// r = 0; -// g = 255; -// b = 0; -// }else{ -// r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); -// g = 0;//255*((xi+1) & 1); -// b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1); -// } - - r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); - g = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); - b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1); - - unsigned int nr_data = X.cols(); - cloud->points.clear(); - for(unsigned int i = 0; i < nr_data; i++){ - pcl::PointXYZRGBNormal p; - p.x = X(0,i); - p.y = X(1,i); - p.z = X(2,i); - p.b = r; - p.g = g; - p.r = b; - cloud->points.push_back(p); - } - - //printf("cloud->points: %i\n",cloud->points.size()); - - char buf [1024]; - sprintf(buf,"cloud%i",xi); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); + viewer->removeAllPointClouds(); + srand(0); + for(unsigned int xi = 0; xi < Xv.size(); xi++){ + + Eigen::MatrixXd X = Xv[xi]; + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + int r,g,b; + + r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); + g = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); + b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1); + + unsigned int nr_data = X.cols(); + cloud->points.clear(); + for(unsigned int i = 0; i < nr_data; i++){ + pcl::PointXYZRGBNormal p; + p.x = X(0,i); + p.y = X(1,i); + p.z = X(2,i); + p.b = r; + p.g = g; + p.r = b; + cloud->points.push_back(p); } + //printf("cloud->points: %i\n",cloud->points.size()); + char buf [1024]; + sprintf(buf,"cloud%i",xi); + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); - if(!save){ - viewer->spin(); - }else{ - viewer->spinOnce(); - } - //void pcl::visualization::PCLVisualizerInteractorStyle::saveScreenshot ( const std::string & file ) - if(save){ - printf("saving: %s\n",filename.c_str()); - viewer->saveScreenshot(filename); - } - viewer->removeAllPointClouds(); + } + + + + if(!save){ + viewer->spin(); + }else{ + viewer->spinOnce(); + } + //void pcl::visualization::PCLVisualizerInteractorStyle::saveScreenshot ( const std::string & file ) + if(save){ + printf("saving: %s\n",filename.c_str()); + viewer->saveScreenshot(filename); + } + viewer->removeAllPointClouds(); //} } + +void MassRegistration::savePCD(std::vector Xv, std::string path){ + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + srand(0); + for(unsigned int xi = 0; xi < Xv.size(); xi++){ + Eigen::MatrixXd X = Xv[xi]; + int r,g,b; + r = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); + g = 256*(1+(rand()%4))/4 - 1;//255*((xi+1) & 1); + b = 256*(1+(rand()%4))/4 - 1;//255*(xi & 1); + + unsigned int nr_data = X.cols(); + for(unsigned int i = 0; i < nr_data; i++){ + pcl::PointXYZRGBNormal p; + p.x = X(0,i); + p.y = X(1,i); + p.z = X(2,i); + p.b = r; + p.g = g; + p.r = b; + cloud->points.push_back(p); + } + } + cloud->width = cloud->points.size(); + cloud->height = 1; + + //pcl::io::savePCDFileBinaryCompressed (cloud,path); + pcl::io::savePCDFileBinaryCompressed (path, *cloud); +} /* void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){ @@ -155,11 +176,11 @@ void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){ scloud->points.clear(); dcloud->points.clear(); for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);} - for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);} + for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);} viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); //printf("pre\n"); - viewer->spin(); + viewer->spin(); //printf("post\n"); viewer->removeAllPointClouds(); @@ -228,7 +249,7 @@ void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y, Eigen::VectorXd W) viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); //printf("pre spin\n"); - viewer->spin(); + viewer->spin(); //printf("post spin\n"); viewer->removeAllPointClouds(); } diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 81159a7a..5caaf3e8 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -3066,6 +3066,12 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector Xv; + for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + savePCD(Xv,savePath+"_before.pcd"); + } + rematch_time = 0; residuals_time = 0; opt_time = 0; @@ -3230,6 +3236,13 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector Xv; + for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);} + savePCD(Xv,savePath+"_after.pcd"); + } + return MassFusionResults(poses,-1); } diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp index e6bb66b5..ef3732bc 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2.cpp @@ -7,6 +7,8 @@ DistanceWeightFunction2::DistanceWeightFunction2(){ p = 0.5; f = PNORM; convergence_threshold = 0.0001; + savePath = ""; + saveData.str(""); } DistanceWeightFunction2::~DistanceWeightFunction2(){} diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR.cpp index d847a6c9..24fdfe4d 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR.cpp @@ -3,6 +3,9 @@ namespace reglib{ DistanceWeightFunction2PPR::DistanceWeightFunction2PPR( double maxd_, int histogram_size_){ + savePath = ""; + saveData.str(""); + update_size = true; start_maxd = maxd_; diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp index 4e582248..bd82fee4 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp @@ -3,6 +3,9 @@ namespace reglib{ DistanceWeightFunction2PPR2::DistanceWeightFunction2PPR2( double maxd_, int histogram_size_){ + savePath = ""; + saveData.str(""); + fixed_histogram_size = false; diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp index cdb1651c..90da4362 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp @@ -15,6 +15,8 @@ double getTime3(){ namespace reglib{ DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3(Distribution * dist_, double maxd_, int histogram_size_){ + savePath = ""; + saveData.str(""); fixed_histogram_size = false; @@ -201,6 +203,15 @@ void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){ double start_time; if(debugg_print){printf("\n%%################################################### ITER:%i ############################################################\n",iter);} + + if( savePath.size() != 0 && iter == 0){ saveData.str("");} + + if( savePath.size() != 0){ + saveData << "\n%################################################### ITER:" << iter << " ############################################################\n"; + } + + + if(!fixed_histogram_size){ if(first){ @@ -285,6 +296,45 @@ void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){ if(debugg_print){printf("figure(%i);",iter+1);} if(debugg_print){printf("clf; hold on; plot(hist_smooth,'r','LineWidth',2); plot(hist,'m','LineWidth',2); plot(noise,'b','LineWidth',2); plot(prob*max(noise),'g','LineWidth',2); plot(infront*max(noise),'g','LineWidth',2);\n");} + if( savePath.size() != 0){ + saveData << "d = ["; + for(int k = 0; k < histogram_size; k++){saveData << (maxd-mind)*double(k)/double(histogram_size-1)+mind << " ";} + saveData << "];\n"; + + saveData << "hist = ["; + for(int k = 0; k < histogram_size; k++){saveData << histogram[k] << " ";} + saveData << "];\n"; + saveData << "hist_smooth = ["; + for(int k = 0; k < histogram_size; k++){saveData << blur_histogram[k] << " ";} + saveData << "];\n"; + saveData << "noise = ["; + for(int k = 0; k < histogram_size; k++){saveData << noise[k] << " ";} + saveData << "];\n"; + saveData << "noisecdf = ["; + for(int k = 0; k < histogram_size; k++){saveData << noisecdf[k] << " ";} + saveData << "];\n"; + saveData << "prob = ["; + for(int k = 0; k < histogram_size; k++){saveData << prob[k] << " ";} + saveData << "];\n"; + saveData << "infront = ["; + for(int k = 0; k < histogram_size; k++){saveData << infront[k] << " ";} + saveData << "];\n"; + saveData << "figure(" << iter+1 <<");\n"; + saveData << "clf; hold on;\n"; + saveData << "title('Distribution of residuals and corresponding estimates')\n"; + saveData << "xlabel('residual values')\n"; + saveData << "ylabel('number of data')\n"; + + saveData << "plot(d,hist_smooth,'r','LineWidth',2);\n"; + saveData << "plot(d,hist,'m','LineWidth',2);\n"; + saveData << "plot(d,noise,'b','LineWidth',2);\n"; + saveData << "plot(prob*max(noise),'g','LineWidth',2);\n"; + if(compute_infront){saveData << "plot(d,infront*max(noise),'c','LineWidth',2);\n";} + saveData << "legend('histogram smoothed','histogram raw','noise estimate','P(Inlier)'"; + if(compute_infront){saveData << ",'P(Infront)'";} + saveData << ");\n"; + } + if(!fixed_histogram_size && update_size ){ double next_maxd; @@ -330,7 +380,22 @@ void DistanceWeightFunction2PPR3::computeModel(MatrixXd mat){ iter++; computeModel(mat); return; - }else{iter = 0;} + }else{ + iter = 0; + if( savePath.size() != 0){ + std::ofstream myfile; + myfile.open (savePath); + myfile << saveData.str(); + myfile.close(); + } + } + }else{ + if( savePath.size() != 0){ + std::ofstream myfile; + myfile.open (savePath); + myfile << saveData.str(); + myfile.close(); + } } } From 057bf7390051c41186e8706eceb2c53d6195f7d6 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 1 Nov 2016 10:05:24 +0100 Subject: [PATCH 176/255] added some more saving of output --- .../src/modelupdater/ModelUpdater.cpp | 24 +++++++++++++------ 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 94d2b28c..ecbd7593 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2725,10 +2725,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_priors.png", priorsimg ); cv::imwrite( savePath+"/segment_"+std::to_string(segment_run_counter)+"_frame_"+std::to_string(i)+"_labelimg.png", labelimg ); } - - - - delete g; Eigen::Matrix4d p = poses[i]; @@ -2745,9 +2741,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s point.x = m00*x + m01*y + m02*z + m03; point.y = m10*x + m11*y + m12*z + m13; point.z = m20*x + m21*y + m22*z + m23; - point.r = priors[3*(offset+ind)+0]*255.0; - point.g = priors[3*(offset+ind)+1]*255.0; - point.b = priors[3*(offset+ind)+2]*255.0; + point.r = frame->rgb.data[3*ind+2];//priors[3*(offset+ind)+0]*255.0; + point.g = frame->rgb.data[3*ind+1];//priors[3*(offset+ind)+1]*255.0; + point.b = frame->rgb.data[3*ind+0];//priors[3*(offset+ind)+2]*255.0; cloud->points.push_back(point); } } @@ -3146,6 +3142,20 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_clusters.pcd", *cloud_sample); + cloud->width = cloud->points.size(); + cloud->height = 1; + pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_full.pcd", *cloud); + + cloud_sample->points.resize(0); + for(unsigned int c = 0; c < sr.component_dynamic.size(); c++){ + for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){ + cloud_sample->points.push_back(cloud->points[sr.component_dynamic[c][i]]); + } + } + cloud_sample->width = cloud_sample->points.size(); + cloud_sample->height = 1; + pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_dynamicobjects.pcd", *cloud_sample); + } if(debugg){ From 9c15663cfa309243c2be9552e1d688c56811c921 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 1 Nov 2016 13:48:46 +0100 Subject: [PATCH 177/255] ... --- quasimodo/quasimodo_brain/CMakeLists.txt | 4 + .../src/pcl_multivisualizer.cpp | 130 ++++++++++++++++++ .../src/modelupdater/ModelUpdater.cpp | 33 +++-- 3 files changed, 157 insertions(+), 10 deletions(-) create mode 100644 quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 6f39fdf4..9bdf1df1 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -142,6 +142,10 @@ add_executable( AnnotationTool src/AnnotationTool.cpp) add_dependencies( AnnotationTool roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp) target_link_libraries( AnnotationTool quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +add_executable( pcl_multivisualizer src/pcl_multivisualizer.cpp) +add_dependencies( pcl_multivisualizer roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp) +target_link_libraries( pcl_multivisualizer quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + ############# ## Install ## diff --git a/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp b/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp new file mode 100644 index 00000000..8d53e5fd --- /dev/null +++ b/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp @@ -0,0 +1,130 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + + + + +int main(int argc, char** argv){ + boost::shared_ptr viewer; + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (1.0, 1.0, 1.0); + //viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + + std::vector< std::vector< pcl::PointCloud::Ptr > > clouds; + for(int i = 1; i < argc;i++){ + printf("input: %s\n",argv[i]); + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + + if (pcl::io::loadPCDFile (argv[i], *cloud) != -1){ + printf("%i\n",cloud->points.size()); + clouds.push_back(std::vector< pcl::PointCloud::Ptr >()); + clouds.back().push_back(cloud); + } + } + printf("nr: %i ->",clouds.size()); + double square = sqrt(double(clouds.size())/1.5); + double wgrid = std::ceil(square*1.5); + double hgrid = std::ceil(double(clouds.size())/double(wgrid)); + printf("square: %f -> w: %f h: %f\n",square,wgrid,hgrid); + + std::vector inds; + for( double w = 0; w+0.5 < wgrid; w++ ){ + for( double h = 0; h+0.5 < hgrid; h++ ){ + int v1(0); + viewer->createViewPort ( double(w+0)/wgrid, double(h+0)/hgrid, double(w+1)/wgrid, double(h+1)/hgrid, v1); + double bgcr = 1;//0.1*double(rand()%10); + double bgcg = 1;//0.1*double(rand()%10); + double bgcb = 1;//0.1*double(rand()%10); + +// printf("%3.3f %3.3f %3.3f %3.3f -> %3.3f %3.3f %3.3f\n", double(w+0)/wgrid, double(h+0)/hgrid, double(w+1)/wgrid, double(h+1)/hgrid, bgcr,bgcg,bgcb); + + viewer->setBackgroundColor (bgcr,bgcg,bgcb, v1); + inds.push_back(v1); +// viewer->spin(); + } + } + + for( int i = 0; i < clouds.size(); i++ ){ + for( int j = 0; j < clouds[i].size(); j++ ){ + viewer->addPointCloud (clouds[i][j], pcl::visualization::PointCloudColorHandlerRGBField(clouds[i][j]), std::to_string(i)+"_"+std::to_string(j),inds[i]); + } + + viewer->addText (argv[1+i], 10, 10,0,0,0, std::to_string(i), inds[i]); + printf("%i -> %i\n",i,inds[i]); + viewer->spin(); + + } + + viewer->spin(); + + for( int i = 0; i < clouds.size(); i++ ){ + viewer->removeAllPointClouds(inds[i]); + + } + + int v1(0); + viewer->createViewPort ( 0, 0, 1, 1, v1); + + for( int i = 0; i < clouds.size(); i++ ){ + for( int j = 0; j < clouds[i].size(); j++ ){ + viewer->addPointCloud (clouds[i][j], pcl::visualization::PointCloudColorHandlerRGBField(clouds[i][j]), std::to_string(i)+"_"+std::to_string(j),v1); + } + //viewer->spinOnce(); + // } + // //void pcl::visualization::PCLVisualizerInteractorStyle::saveScreenshot ( const std::string & file ) + // if(save){ + // printf("saving: %s\n",filename.c_str()); + // viewer->saveScreenshot(filename); + viewer->spin(); + viewer->removeAllPointClouds(v1); + } + + + +// viewer->addText ("Radius: 0.01", 10, 10, "v1 text", v1); +// pcl::visualization::PointCloudColorHandlerRGBField rgb (cloud); +// viewer->addPointCloud (cloud, rgb, "sample cloud1", v1); + +// int v2(0); +// viewer->createViewPort (0.5, 0.0, 1.0, 1.0, v2); +// viewer->setBackgroundColor (0.3, 0.3, 0.3, v2); +// viewer->addText ("Radius: 0.1", 10, 10, "v2 text", v2); +// pcl::visualization::PointCloudColorHandlerCustom single_color (cloud, 0, 255, 0); +// viewer->addPointCloud (cloud, single_color, "sample cloud2", v2); + + + + viewer->spin(); + +// if(!save){ + +// }else{ +// +// } +// viewer->removeAllPointClouds(); + + printf("done...\n"); + return 0; +} diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index ecbd7593..c27c99bf 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -3073,12 +3073,16 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s cloud_sample->width = current_point; cloud_sample->height = 1; for(unsigned int i = 0; i < current_point; i++){ + cloud_sample->points[i] = cloud->points[i]; cloud_sample->points[i].r = priors[3*i+0]*255.0; cloud_sample->points[i].g = priors[3*i+1]*255.0; cloud_sample->points[i].b = priors[3*i+2]*255.0; } pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_priors.pcd", *cloud_sample); +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); +// viewer->spin(); for(unsigned int i = 0; i < current_point; i++){ cloud_sample->points[i].r = 0; @@ -3102,6 +3106,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_classes.pcd", *cloud_sample); +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); +// viewer->spin(); for(unsigned int i = 0; i < current_point; i++){ cloud_sample->points[i].r = 0; @@ -3113,11 +3120,11 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s int randr = rand()%256; int randg = rand()%256; int randb = rand()%256; - if(randr*randr + randg*randg + (255-randr)*(255-randr) < 1000){ - randr = rand()%256; - randg = rand()%256; - randb = rand()%256; - } +// if(randr*randr + randg*randg + (255-randr)*(255-randr) < 1000){ +// randr = rand()%256; +// randg = rand()%256; +// randb = rand()%256; +// } for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){ cloud_sample->points[sr.component_dynamic[c][i]].r = randr; cloud_sample->points[sr.component_dynamic[c][i]].g = randg; @@ -3129,11 +3136,11 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s int randr = rand()%256; int randg = rand()%256; int randb = rand()%256; - if(randr*randr + randg*randg + (255-randr)*(255-randr) < 1000){ - randr = rand()%256; - randg = rand()%256; - randb = rand()%256; - } +// if(randr*randr + randg*randg + (255-randr)*(255-randr) < 1000){ +// randr = rand()%256; +// randg = rand()%256; +// randb = rand()%256; +// } for(unsigned int i = 0; i < sr.component_moving[c].size(); i++){ cloud_sample->points[sr.component_moving[c][i]].r = randr; cloud_sample->points[sr.component_moving[c][i]].g = randg; @@ -3141,6 +3148,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_clusters.pcd", *cloud_sample); +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); +// viewer->spin(); cloud->width = cloud->points.size(); cloud->height = 1; @@ -3155,6 +3165,9 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s cloud_sample->width = cloud_sample->points.size(); cloud_sample->height = 1; pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_dynamicobjects.pcd", *cloud_sample); +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); +// viewer->spin(); } From f9f0fd503f636c4f2619411ba672bf69cc3949d8 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 1 Nov 2016 16:26:48 +0100 Subject: [PATCH 178/255] ... --- .../launch/learn_objects_dependencies.launch | 2 + .../src/pcl_multivisualizer.cpp | 62 +++++++------------ 2 files changed, 25 insertions(+), 39 deletions(-) diff --git a/learn_objects_action/launch/learn_objects_dependencies.launch b/learn_objects_action/launch/learn_objects_dependencies.launch index edacc765..e6b067bc 100644 --- a/learn_objects_action/launch/learn_objects_dependencies.launch +++ b/learn_objects_action/launch/learn_objects_dependencies.launch @@ -15,6 +15,8 @@ + + diff --git a/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp b/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp index 8d53e5fd..ec295512 100644 --- a/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp +++ b/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp @@ -54,18 +54,19 @@ int main(int argc, char** argv){ for( double h = 0; h+0.5 < hgrid; h++ ){ int v1(0); viewer->createViewPort ( double(w+0)/wgrid, double(h+0)/hgrid, double(w+1)/wgrid, double(h+1)/hgrid, v1); - double bgcr = 1;//0.1*double(rand()%10); - double bgcg = 1;//0.1*double(rand()%10); - double bgcb = 1;//0.1*double(rand()%10); - -// printf("%3.3f %3.3f %3.3f %3.3f -> %3.3f %3.3f %3.3f\n", double(w+0)/wgrid, double(h+0)/hgrid, double(w+1)/wgrid, double(h+1)/hgrid, bgcr,bgcg,bgcb); - - viewer->setBackgroundColor (bgcr,bgcg,bgcb, v1); inds.push_back(v1); -// viewer->spin(); } } +// int v1(0); +// viewer->createViewPort ( 0, 0, 1, 1, v1); +// viewer->spin(); +// viewer->setBackgroundColor (1.0,1.0,1.0, v1); +// viewer->spin(); +// viewer->createViewPort ( 0, 0, 1, 1, v1); +// viewer->spin(); +// viewer->setBackgroundColor (1.0,1.0,1.0, v1); + for( int i = 0; i < clouds.size(); i++ ){ for( int j = 0; j < clouds[i].size(); j++ ){ viewer->addPointCloud (clouds[i][j], pcl::visualization::PointCloudColorHandlerRGBField(clouds[i][j]), std::to_string(i)+"_"+std::to_string(j),inds[i]); @@ -73,57 +74,40 @@ int main(int argc, char** argv){ viewer->addText (argv[1+i], 10, 10,0,0,0, std::to_string(i), inds[i]); printf("%i -> %i\n",i,inds[i]); - viewer->spin(); + viewer->setBackgroundColor (1.0,1.0,1.0, inds[i]+1); } viewer->spin(); + for( int i = 0; i < clouds.size(); i++ ){ viewer->removeAllPointClouds(inds[i]); - } int v1(0); viewer->createViewPort ( 0, 0, 1, 1, v1); + viewer->setBackgroundColor (255.0, 255.0, 255.0,v1+1); for( int i = 0; i < clouds.size(); i++ ){ for( int j = 0; j < clouds[i].size(); j++ ){ viewer->addPointCloud (clouds[i][j], pcl::visualization::PointCloudColorHandlerRGBField(clouds[i][j]), std::to_string(i)+"_"+std::to_string(j),v1); } - //viewer->spinOnce(); - // } - // //void pcl::visualization::PCLVisualizerInteractorStyle::saveScreenshot ( const std::string & file ) - // if(save){ - // printf("saving: %s\n",filename.c_str()); - // viewer->saveScreenshot(filename); - viewer->spin(); + viewer->spinOnce(); + std::string path = std::string(argv[1+i]); + path.pop_back(); + path.pop_back(); + path.pop_back(); + path += "png"; + printf("path: %s\n",path.c_str()); + + viewer->saveScreenshot(path); + viewer->spinOnce(); + //viewer->spin(); viewer->removeAllPointClouds(v1); } - - - -// viewer->addText ("Radius: 0.01", 10, 10, "v1 text", v1); -// pcl::visualization::PointCloudColorHandlerRGBField rgb (cloud); -// viewer->addPointCloud (cloud, rgb, "sample cloud1", v1); - -// int v2(0); -// viewer->createViewPort (0.5, 0.0, 1.0, 1.0, v2); -// viewer->setBackgroundColor (0.3, 0.3, 0.3, v2); -// viewer->addText ("Radius: 0.1", 10, 10, "v2 text", v2); -// pcl::visualization::PointCloudColorHandlerCustom single_color (cloud, 0, 255, 0); -// viewer->addPointCloud (cloud, single_color, "sample cloud2", v2); - - - viewer->spin(); -// if(!save){ - -// }else{ -// -// } -// viewer->removeAllPointClouds(); printf("done...\n"); return 0; From 89c966566e34bffc6eb917bcfbb09c49544a6522 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 2 Nov 2016 23:02:58 +0100 Subject: [PATCH 179/255] ... --- quasimodo/quasimodo_brain/CMakeLists.txt | 4 + .../metaroom_additional_view_processing.cpp | 101 +++++++++ .../src/pcl_multivisualizer.cpp | 10 +- quasimodo/quasimodo_brain/src/zoomer.cpp | 191 ++++++++++++++++++ .../quasimodo_models/src/core/RGBDFrame.cpp | 2 +- .../quasimodo_models/src/model/Model.cpp | 8 +- .../src/modelupdater/ModelUpdater.cpp | 10 +- 7 files changed, 311 insertions(+), 15 deletions(-) create mode 100644 quasimodo/quasimodo_brain/src/zoomer.cpp diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 9bdf1df1..c4d02447 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -146,6 +146,10 @@ add_executable( pcl_multivisualizer src/pcl_multivisualizer.cpp) add_dependencies( pcl_multivisualizer roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp) target_link_libraries( pcl_multivisualizer quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +add_executable( zoomer src/zoomer.cpp) +add_dependencies( zoomer roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp) +target_link_libraries( zoomer quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) + ############# ## Install ## diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index dda3be64..c66e4eea 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -568,6 +568,92 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri } } + { + reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); + std::vector frames; + std::vector masks; + std::vector unrefined; + + std::vector both_unrefined; + //both_unrefined.push_back(Eigen::Matrix4d::Identity()); + + + + std::vector times; + for(unsigned int i = 0; i < 3000 && i < viewrgbs.size(); i++){ + printf("additional view: %i\n",i); + geometry_msgs::TransformStamped msg; + tf::transformStampedTFToMsg(viewtfs[i], msg); + long sec = msg.header.stamp.sec; + long nsec = msg.header.stamp.nsec; + double time = double(sec)+1e-9*double(nsec); + + Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]); + + cout << m << endl << endl; + + unrefined.push_back(m); + times.push_back(time); + + cv::Mat fullmask; + fullmask.create(480,640,CV_8UC1); + unsigned char * maskdata = (unsigned char *)fullmask.data; + for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} + masks.push_back(new reglib::ModelMask(fullmask)); + + reglib::Camera * cam = sweep->frames.front()->camera->clone(); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m,true,savePath);//a.matrix()); + frames.push_back(frame); + + both_unrefined.push_back(quasimodo_brain::getMat(viewtfs[0]).inverse()*m); + } + + + if(frames.size() > 0){ + + + reglib::Model * fullmodel = new reglib::Model(); + fullmodel->points.clear(); + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.25); + if(savePath.size() != 0){ + bgmassreg->savePath = savePath+"/processAV_"+std::to_string(fullmodel->id); + } + + bgmassreg->timeout = 300; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = false; + bgmassreg->visualizationLvl = visualization_lvl_regref; + + bgmassreg->maskstep = 2; + bgmassreg->nomaskstep = 2; + + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + //bgmassreg->addModel(sweep); + bgmassreg->setData(frames,masks); + + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); + delete bgmassreg; + + for(unsigned int i = 0; i < frames.size(); i++){ + fullmodel->frames.push_back(frames[i]); + fullmodel->modelmasks.push_back(masks[i]); + fullmodel->relativeposes.push_back(bgmfr.poses[i]); + } + fullmodel->recomputeModelPoints(); + + if(savePath.size() != 0){ + pcl::PointCloud::Ptr cld = fullmodel->getPCLnormalcloud(1, false); + pcl::io::savePCDFileBinaryCompressed (savePath+"/processAV_"+std::to_string(fullmodel->id)+"_fused.pcd", *cld); + } + } + + + + exit(0); + } + reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; @@ -668,14 +754,22 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri bgmassreg->use_surface = true; bgmassreg->use_depthedge = false; bgmassreg->visualizationLvl = visualization_lvl_regref; +// bgmassreg->maskstep = 5; +// bgmassreg->nomaskstep = 5; + bgmassreg->maskstep = 5; bgmassreg->nomaskstep = 5; + bgmassreg->nomask = true; bgmassreg->stopval = 0.0005; bgmassreg->addModel(sweep); bgmassreg->setData(frames,masks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); + + //printf("%s\n",bgmassreg->savePath.c_str()); + //exit(0); + delete bgmassreg; // reglib::MassRegistrationPPR2 * bgmassreg2 = new reglib::MassRegistrationPPR2(0.01); @@ -703,6 +797,13 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); } fullmodel->recomputeModelPoints(); + + if(savePath.size() != 0){ + pcl::PointCloud::Ptr cld = fullmodel->getPCLnormalcloud(1, false); + pcl::io::savePCDFileBinaryCompressed (savePath+"/processAV_"+std::to_string(fullmodel->id)+"_fused.pcd", *cld); + } + //pcl::PointCloud::Ptr Model::getPCLnormalcloud(int step, bool color){ + }else{ fullmodel->points = sweep->points; } diff --git a/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp b/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp index ec295512..0c8f88af 100644 --- a/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp +++ b/quasimodo/quasimodo_brain/src/pcl_multivisualizer.cpp @@ -58,15 +58,6 @@ int main(int argc, char** argv){ } } -// int v1(0); -// viewer->createViewPort ( 0, 0, 1, 1, v1); -// viewer->spin(); -// viewer->setBackgroundColor (1.0,1.0,1.0, v1); -// viewer->spin(); -// viewer->createViewPort ( 0, 0, 1, 1, v1); -// viewer->spin(); -// viewer->setBackgroundColor (1.0,1.0,1.0, v1); - for( int i = 0; i < clouds.size(); i++ ){ for( int j = 0; j < clouds[i].size(); j++ ){ viewer->addPointCloud (clouds[i][j], pcl::visualization::PointCloudColorHandlerRGBField(clouds[i][j]), std::to_string(i)+"_"+std::to_string(j),inds[i]); @@ -92,6 +83,7 @@ int main(int argc, char** argv){ for( int i = 0; i < clouds.size(); i++ ){ for( int j = 0; j < clouds[i].size(); j++ ){ viewer->addPointCloud (clouds[i][j], pcl::visualization::PointCloudColorHandlerRGBField(clouds[i][j]), std::to_string(i)+"_"+std::to_string(j),v1); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, std::to_string(i)+"_"+std::to_string(j),v1); } viewer->spinOnce(); std::string path = std::string(argv[1+i]); diff --git a/quasimodo/quasimodo_brain/src/zoomer.cpp b/quasimodo/quasimodo_brain/src/zoomer.cpp new file mode 100644 index 00000000..bd24051a --- /dev/null +++ b/quasimodo/quasimodo_brain/src/zoomer.cpp @@ -0,0 +1,191 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + + +#include +#include +#include "opencv2/opencv.hpp" +#include + +int thickness = 3; + +std::vector< cv::Mat > imgs; +int state = 0; + +double corner1_x = 0; +double corner1_y = 0; + +double corner2_x = 0; +double corner2_y = 0; + +double corner3_x = 0; +double corner3_y = 0; + +double corner4_x = 0; +double corner4_y = 0; +void CallBackFunc(int event, int x, int y, int flags, void* userdata){ + if ( event == cv::EVENT_LBUTTONDOWN ){ + std::cout << "Left button of the mouse is clicked - position (" << x << ", " << y << ")" << endl; + if(state == 0){ + state = 1; + corner1_x = x; + corner1_y = y; + }else if(state == 1){ + state = 2; + + if(x > corner1_x){ + corner2_x = x; + }else{ + corner2_x = corner1_x; + corner1_x = x; + } + + if(y > corner1_y){ + corner2_y = y; + }else{ + corner2_y = corner1_y; + corner1_y = y; + } + }else if(state == 2){ + state = 3; + corner3_x = x; + corner3_y = y; + }else if(state == 3){ + state = 4; + corner4_x = x; + corner4_y = y; + } + }else if ( event == cv::EVENT_RBUTTONDOWN ){ + //std::cout << "Right button of the mouse is clicked - position (" << x << ", " << y << ")" << endl; + }else if ( event == cv::EVENT_MBUTTONDOWN ){ + //std::cout << "Middle button of the mouse is clicked - position (" << x << ", " << y << ")" << endl; + }else if ( event == cv::EVENT_MOUSEMOVE ){ + //std::cout << "Mouse move over the window - position (" << x << ", " << y << ")" << endl; + } + + if(state == 5){return;} + //printf("state: %i\n",state); + if(state >= 1){ + cv::Mat m = imgs.front().clone(); + + if(state >= 2){ + cv::Rect ROI1(cv::Point(corner1_x,corner1_y),cv::Point(corner2_x,corner2_y)); + cv::rectangle(m, ROI1 , cv::Scalar(0,0,255),thickness); + + // cv::line(m, cv::Point(x, y), cv::Point(corner1_x, corner1_y), cv::Scalar(0,255,0),thickness); + // cv::line(m, cv::Point(x, y), cv::Point(corner2_x, corner2_y), cv::Scalar(0,255,0),thickness); + + if(state >= 3){ + + double ratio = fabs(corner1_x-corner2_x)/fabs(corner1_y-corner2_y); + //printf("ratio: %f\n",ratio); + double offsets = fabs(std::max(fabs(corner3_x-x)/ratio,fabs(corner3_y-y))); + offsets = std::max(offsets,1.0); + + + double c1x = corner3_x+offsets*ratio; + double c1y = corner3_y+offsets; + double c2x = corner3_x-offsets*ratio; + double c2y = corner3_y-offsets; + + double midx = 0.5*(corner1_x+corner2_x); + double midy = 0.5*(corner1_y+corner2_y); + + + + cv::Point a = cv::Point(c2x, c2y); + cv::Point b = cv::Point(corner1_x, corner1_y); + +// if( c2x < std::min(corner1_x,corner2_x )){// && c1y < std::min(corner1_y,corner2_y )){ +// a.x = c2x; +// a.y = c1y; +// } + +// if (c2x < corner1_x){ +// a.y = cv::Point(c2x, c1y); +//// cv::line(m, cv::Point(c2x, c1y), cv::Point(corner1_x, corner1_y), cv::Scalar(0,255,0),thickness); +//// }else if(c2x > corner1_x && c2y < corner1_y){ +//// cv::line(m, cv::Point(c2x, c2y), cv::Point(corner1_x, corner1_y), cv::Scalar(255,0,0),thickness); +// }else{ +// //cv::line(m, cv::Point(c2x, c2y), cv::Point(corner1_x, corner1_y), cv::Scalar(0,0,255),thickness); +// a = cv::Point(c2x, c2y); +// } + + + //cv::line(m, cv::Point(c2x, c1y), cv::Point(corner1_x, corner1_y), cv::Scalar(0,255,0),thickness); + cv::line(m, a, b, cv::Scalar(0,255,0),thickness); + cv::line(m, cv::Point(c1x, c1y), cv::Point(corner2_x, corner2_y), cv::Scalar(0,0,255),thickness); + + cv::Rect ROI2(cv::Point(c1x,c1y),cv::Point(c2x, c2y)); + cv::rectangle(m, ROI2 , cv::Scalar(255,0,255),thickness); + + if(state >= 4){ + state = 5; + cv::Mat localimg = imgs.front()(ROI1); + + cv::Mat outimg; + outimg.create(ROI2.height, ROI2.width,CV_8UC3); + cv::resize(localimg, outimg, outimg.size(),0,0,cv::INTER_AREA); + + outimg.copyTo(m(ROI2)); + cv::rectangle(m, ROI2 , cv::Scalar(0,0,255),thickness); + + cv::imshow("Image", m); + cv::waitKey(0); + } + }else{ + cv::line(m, cv::Point(x, y), cv::Point(corner1_x, corner1_y), cv::Scalar(0,255,0),thickness); + cv::line(m, cv::Point(x, y), cv::Point(corner2_x, corner2_y), cv::Scalar(0,255,0),thickness); + } + }else{ + cv::Rect ROI1(cv::Point(corner1_x,corner1_y),cv::Point(x,y)); + cv::rectangle(m, ROI1 , cv::Scalar(255,0,255),thickness); + } + cv::imshow("Image", m); + cv::waitKey(0); + } +} + +int main(int argc, char** argv){ + + for(int i = 1; i < argc;i++){ + printf("input: %s\n",argv[i]); + cv::Mat m = cv::imread(argv[i], CV_LOAD_IMAGE_UNCHANGED); + if(m.data ){ + cv::namedWindow( "Image", cv::WINDOW_AUTOSIZE ); + cv::imshow( "Image", m ); + cv::waitKey(30); + imgs.push_back(m); + } + } + + if(imgs.size() > 0){ + cv::namedWindow("Image", 1); + cv::setMouseCallback("Image", CallBackFunc, NULL); + cv::imshow("Image", imgs.front()); + cv::waitKey(0); + } + + return 0; +} diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 9cb45e6c..7018cac5 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -1373,7 +1373,7 @@ void RGBDFrame::saveCombinedProcessedImages(std::string path){ for(unsigned long h = 0; h < height;h++){ for(unsigned long w = 0; w < width;w++){ - if(normalsdata[3*(h*width+w)+0] > 1){ + if(normalsdata[3*(h*width+w)+0] <= 1){ combineddata[3*(h*(3*width)+w+0*width)+0] = 255.0*fabs(normalsdata[3*(h*width+w)+0]); combineddata[3*(h*(3*width)+w+0*width)+1] = 255.0*fabs(normalsdata[3*(h*width+w)+1]); combineddata[3*(h*(3*width)+w+0*width)+2] = 255.0*fabs(normalsdata[3*(h*width+w)+2]); diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 7e407d09..57fde321 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -564,9 +564,9 @@ pcl::PointCloud::Ptr Model::getPCLnormalcloud(int step, p.g = 255; p.r = 0; }else{ - p.r = sp.feature(0); + p.b = sp.feature(0); p.g = sp.feature(1); - p.b = sp.feature(2); + p.r = sp.feature(2); } cloud_ptr->points.push_back(p); } @@ -587,9 +587,9 @@ pcl::PointCloud::Ptr Model::getPCLcloud(int step, bool color){ p.g = 255; p.r = 0; }else{ - p.r = sp.feature(0); + p.b = sp.feature(0); p.g = sp.feature(1); - p.b = sp.feature(2); + p.r = sp.feature(2); } cloud_ptr->points.push_back(p); } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index c27c99bf..5329732a 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2330,7 +2330,7 @@ int setupPriors(int method,float current_occlusions, float current_overlaps, flo if(method == 3){ if(valid){ float minprob = 0.01; - float bias = 0.005; + float bias = 0.01; float overlap_same_prob = 0.75; //Prob behind all bg @@ -3023,6 +3023,14 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } + for(unsigned long ind = 0; ind < nr_pixels; ind++){ + unsigned int ol = objectlabel[ind]; + if(ol != 0 && labelID[ol] == 0){ + objectlabel[ind] = 0; + labels[ind] = 0; + } + } + for(unsigned int i = 0; i < interframe_connectionId.size();i++){interframe_connectionId[i].clear();} interframe_connectionId.clear(); From 35af1ca8fa33add11ff2e16eabd8bf113315e2c2 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 3 Nov 2016 11:10:27 +0100 Subject: [PATCH 180/255] added installtargets --- quasimodo/quasimodo_brain/CMakeLists.txt | 4 +- .../metaroom_additional_view_processing.cpp | 159 ++++++++---------- quasimodo/quasimodo_models/CMakeLists.txt | 5 +- 3 files changed, 77 insertions(+), 91 deletions(-) diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index c4d02447..1f13c5a7 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -103,7 +103,7 @@ target_link_libraries( massregPCD quasimodo_ModelDatabase quasimodo_ModelUpdater add_executable( voPCD src/voPCD.cpp) add_dependencies( voPCD roscpp quasimodo_msgs_generate_messages_cpp) -target_link_libraries( voPCD quasimodo_ModelDatabase quasimodo_ModelUpdater image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries( voPCD quasimodo_brain_util quasimodo_ModelDatabase quasimodo_ModelUpdater image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) add_executable( velodyne2 src/velodyne2.cpp) @@ -160,7 +160,7 @@ target_link_libraries( zoomer quasimodo_brain_util quasimodo_ModelDatabase quasi # Mark executables and/or libraries for installation -install(TARGETS quasimodo_ModelDatabase preload_object_data modelserver robot_listener +install(TARGETS zoomer pcl_multivisualizer quasimodo_brain_util quasimodo_ModelDatabase preload_object_data modelserver robot_listener massregPCD voPCD velodyne2 MeshTest test_align2 test_segment segment_room_pairs test_segment_room_pairs segmentationserver metaroom_additional_view_processing AnnotationTool ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index c66e4eea..fd9321da 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -568,91 +568,80 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri } } - { - reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); - std::vector frames; - std::vector masks; - std::vector unrefined; - - std::vector both_unrefined; - //both_unrefined.push_back(Eigen::Matrix4d::Identity()); - - - - std::vector times; - for(unsigned int i = 0; i < 3000 && i < viewrgbs.size(); i++){ - printf("additional view: %i\n",i); - geometry_msgs::TransformStamped msg; - tf::transformStampedTFToMsg(viewtfs[i], msg); - long sec = msg.header.stamp.sec; - long nsec = msg.header.stamp.nsec; - double time = double(sec)+1e-9*double(nsec); - - Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]); - - cout << m << endl << endl; - - unrefined.push_back(m); - times.push_back(time); - - cv::Mat fullmask; - fullmask.create(480,640,CV_8UC1); - unsigned char * maskdata = (unsigned char *)fullmask.data; - for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} - masks.push_back(new reglib::ModelMask(fullmask)); - - reglib::Camera * cam = sweep->frames.front()->camera->clone(); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m,true,savePath);//a.matrix()); - frames.push_back(frame); - - both_unrefined.push_back(quasimodo_brain::getMat(viewtfs[0]).inverse()*m); - } - - - if(frames.size() > 0){ - - - reglib::Model * fullmodel = new reglib::Model(); - fullmodel->points.clear(); - reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.25); - if(savePath.size() != 0){ - bgmassreg->savePath = savePath+"/processAV_"+std::to_string(fullmodel->id); - } - - bgmassreg->timeout = 300; - bgmassreg->viewer = viewer; - bgmassreg->use_surface = true; - bgmassreg->use_depthedge = false; - bgmassreg->visualizationLvl = visualization_lvl_regref; - - bgmassreg->maskstep = 2; - bgmassreg->nomaskstep = 2; - - bgmassreg->nomask = true; - bgmassreg->stopval = 0.0005; - //bgmassreg->addModel(sweep); - bgmassreg->setData(frames,masks); - - reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); - delete bgmassreg; - - for(unsigned int i = 0; i < frames.size(); i++){ - fullmodel->frames.push_back(frames[i]); - fullmodel->modelmasks.push_back(masks[i]); - fullmodel->relativeposes.push_back(bgmfr.poses[i]); - } - fullmodel->recomputeModelPoints(); - - if(savePath.size() != 0){ - pcl::PointCloud::Ptr cld = fullmodel->getPCLnormalcloud(1, false); - pcl::io::savePCDFileBinaryCompressed (savePath+"/processAV_"+std::to_string(fullmodel->id)+"_fused.pcd", *cld); - } - } - - - - exit(0); - } +// { +// reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); +// std::vector frames; +// std::vector masks; +// std::vector unrefined; + +// std::vector both_unrefined; +// //both_unrefined.push_back(Eigen::Matrix4d::Identity()); + +// std::vector times; +// for(unsigned int i = 0; i < 3000 && i < viewrgbs.size(); i++){ +// printf("additional view: %i\n",i); +// geometry_msgs::TransformStamped msg; +// tf::transformStampedTFToMsg(viewtfs[i], msg); +// long sec = msg.header.stamp.sec; +// long nsec = msg.header.stamp.nsec; +// double time = double(sec)+1e-9*double(nsec); + +// Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]); + +// unrefined.push_back(m); +// times.push_back(time); + +// cv::Mat fullmask; +// fullmask.create(480,640,CV_8UC1); +// unsigned char * maskdata = (unsigned char *)fullmask.data; +// for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} +// masks.push_back(new reglib::ModelMask(fullmask)); + +// reglib::Camera * cam = sweep->frames.front()->camera->clone(); +// reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m,true,savePath);//a.matrix()); +// frames.push_back(frame); + +// both_unrefined.push_back(quasimodo_brain::getMat(viewtfs[0]).inverse()*m); +// } + +// if(frames.size() > 0){ +// reglib::Model * fullmodel = new reglib::Model(); +// fullmodel->points.clear(); +// reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.25); +// if(savePath.size() != 0){ +// bgmassreg->savePath = savePath+"/processAV_"+std::to_string(fullmodel->id); +// } + +// bgmassreg->timeout = 300; +// bgmassreg->viewer = viewer; +// bgmassreg->use_surface = true; +// bgmassreg->use_depthedge = false; +// bgmassreg->visualizationLvl = visualization_lvl_regref; + +// bgmassreg->maskstep = 2; +// bgmassreg->nomaskstep = 2; + +// bgmassreg->nomask = true; +// bgmassreg->stopval = 0.0005; +// //bgmassreg->addModel(sweep); +// bgmassreg->setData(frames,masks); + +// reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); +// delete bgmassreg; + +// for(unsigned int i = 0; i < frames.size(); i++){ +// fullmodel->frames.push_back(frames[i]); +// fullmodel->modelmasks.push_back(masks[i]); +// fullmodel->relativeposes.push_back(bgmfr.poses[i]); +// } +// fullmodel->recomputeModelPoints(); + +// if(savePath.size() != 0){ +// pcl::PointCloud::Ptr cld = fullmodel->getPCLnormalcloud(1, false); +// pcl::io::savePCDFileBinaryCompressed (savePath+"/processAV_"+std::to_string(fullmodel->id)+"_fused.pcd", *cld); +// } +// } +// } reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 77159046..976f21f0 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -104,12 +104,9 @@ target_link_libraries(quasimodo_ModelUpdater quasimodo_Mesher quasimodo_model qu # Mark executables and/or libraries for installation -install(TARGETS quasimodo_weightlib quasimodo_core quasimodo_reglib quasimodo_massreglib quasimodo_modelmask quasimodo_model quasimodo_Mesher quasimodo_ModelUpdater +install(TARGETS quasimodo_SignalProcessing quasimodo_distribution quasimodo_weightlib quasimodo_superpoint quasimodo_ReprojectionResult quasimodo_OcclusionScore quasimodo_core quasimodo_reglib quasimodo_massreglib quasimodo_modelmask quasimodo_model quasimodo_Mesher quasimodo_ModelUpdater ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -#install(DIRECTORY launch -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -#) From 8b77347b8a183a37eebd7ad5c01ff3aac3f388e1 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 4 Nov 2016 13:09:18 +0100 Subject: [PATCH 181/255] debugging --- .../include/cloud_merge/cloud_merge_node.h | 6 ++- quasimodo/quasimodo_brain/src/Util/Util.cpp | 50 ++++++++++++++++++- 2 files changed, 53 insertions(+), 3 deletions(-) diff --git a/cloud_merge/include/cloud_merge/cloud_merge_node.h b/cloud_merge/include/cloud_merge/cloud_merge_node.h index 6ee77aef..a39c157d 100644 --- a/cloud_merge/include/cloud_merge/cloud_merge_node.h +++ b/cloud_merge/include/cloud_merge/cloud_merge_node.h @@ -434,7 +434,7 @@ void CloudMergeNode::controlCallback(const std_msgs::String& controlS } if (controlString.data == "end_sweep") - { + { ROS_INFO_STREAM("Pan tilt sweep stopped"); m_bAquisitionPhase = false; @@ -485,6 +485,8 @@ void CloudMergeNode::controlCallback(const std_msgs::String& controlS if (m_bRegisterAndCorrectSweep) { + + printf("LINE: %i\n",__LINE__); // load precalibrated camera poses // std::vector regTransforms = semantic_map_registration_transforms::loadRegistrationTransforms("default", true); std::vector corresponding_registeredPoses; @@ -543,6 +545,8 @@ void CloudMergeNode::controlCallback(const std_msgs::String& controlS } } + printf("LINE: %i\n",__LINE__); + // Pulbish room observation semantic_map::RoomObservation obs_msg; obs_msg.xml_file_name = roomXMLPath; diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index aafc4d5e..efa02505 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -235,6 +235,8 @@ std::vector getRegisteredViewPoses(const std::string& poses_fil } Eigen::Matrix4d getMat(tf::StampedTransform tf){ + + printf("getMat\n"); //Transform geometry_msgs::TransformStamped tfstmsg; tf::transformStampedTFToMsg (tf, tfstmsg); @@ -246,6 +248,8 @@ Eigen::Matrix4d getMat(tf::StampedTransform tf){ pose.position.z = tfmsg.translation.z; Eigen::Affine3d epose; tf::poseMsgToEigen(pose, epose); + + std::cout << epose.matrix() << std::endl << std::endl; return epose.matrix(); } @@ -260,10 +264,48 @@ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath) dummy.push_back("IntermediatePosition"); SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml",dummy); - reglib::Model * sweepmodel = 0; - Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); + +// printf("roomData.vIntermediateRoomCloudTransforms.size(): %i\n",roomData.vIntermediateRoomCloudTransforms.size()); +// for (size_t i=0; ifx = aCameraModel.fx(); +// cam->fy = aCameraModel.fy(); +// cam->cx = aCameraModel.cx(); +// cam->cy = aCameraModel.cy(); +// cam->print(); +// delete cam; +// } + + +// printf("roomData.vIntermediateRoomCloudCamParams.size(): %i\n",roomData.vIntermediateRoomCloudCamParams.size()); +// for (size_t i=0; ifx = aCameraModel.fx(); +// cam->fy = aCameraModel.fy(); +// cam->cx = aCameraModel.cx(); +// cam->cy = aCameraModel.cy(); +// cam->print(); +// delete cam; +// } +//exit(0); + + reglib::Model * sweepmodel = 0; + Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); std::vector current_room_frames; for (size_t i=0; ifx = aCameraModel.fx(); cam->fy = aCameraModel.fy(); @@ -281,6 +324,8 @@ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath) //cam->print(); Eigen::Matrix4d m = m2*getMat(roomData.vIntermediateRoomCloudTransformsRegistered[i]); + //Eigen::Matrix4d m = getMat(roomData.vIntermediateRoomCloudTransforms[i]); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, m,true,savePath); current_room_frames.push_back(frame); @@ -293,6 +338,7 @@ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath) } } + sweepmodel->recomputeModelPoints(); return sweepmodel; From 5fa9c9c41fc6af3f50e386673a3a4bcd2eff3f01 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 4 Nov 2016 13:41:19 +0100 Subject: [PATCH 182/255] ... --- cloud_merge/include/cloud_merge/cloud_merge_node.h | 10 +++------- semantic_map/src/reg_transforms.cpp | 10 +++++++++- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/cloud_merge/include/cloud_merge/cloud_merge_node.h b/cloud_merge/include/cloud_merge/cloud_merge_node.h index a39c157d..d91aa5e3 100644 --- a/cloud_merge/include/cloud_merge/cloud_merge_node.h +++ b/cloud_merge/include/cloud_merge/cloud_merge_node.h @@ -483,10 +483,8 @@ void CloudMergeNode::controlCallback(const std_msgs::String& controlS std::string roomXMLPath = parser.saveRoomAsXML(aSemanticRoom); ROS_INFO_STREAM("Saved semantic room"); - if (m_bRegisterAndCorrectSweep) - { - - printf("LINE: %i\n",__LINE__); + if (m_bRegisterAndCorrectSweep) + { // load precalibrated camera poses // std::vector regTransforms = semantic_map_registration_transforms::loadRegistrationTransforms("default", true); std::vector corresponding_registeredPoses; @@ -543,9 +541,7 @@ void CloudMergeNode::controlCallback(const std_msgs::String& controlS reg.saveOrbFeatures(aSemanticRoom,base_path); } - } - - printf("LINE: %i\n",__LINE__); + } // Pulbish room observation semantic_map::RoomObservation obs_msg; diff --git a/semantic_map/src/reg_transforms.cpp b/semantic_map/src/reg_transforms.cpp index bcd79e29..87b35df6 100644 --- a/semantic_map/src/reg_transforms.cpp +++ b/semantic_map/src/reg_transforms.cpp @@ -372,18 +372,23 @@ std::vector semantic_map_registration_transforms::loadCorr std::string sweep_params_file, bool verbose) { +printf("%s::%i\n",__FILE__,__LINE__); // load calibrated data std::vector allTransforms = semantic_map_registration_transforms::loadRegistrationTransforms(transforms_file, verbose); +printf("%s::%i\n",__FILE__,__LINE__); std::vector empty; std::vector toRet; SweepParameters calibrate_sweep_parameters; +printf("%s::%i\n",__FILE__,__LINE__); semantic_map_registration_transforms::loadSweepParameters(calibrate_sweep_parameters, verbose, sweep_params_file); - +printf("%s::%i\n",__FILE__,__LINE__); // find matching transforms for current sweep parameters from calibrate sweep parameters int number_of_positions = my_sweep_params.getNumberOfIntermediatePositions(); for (int k=0; k semantic_map_registration_transforms::loadCorr int my_pan, my_tilt, their_pan, their_tilt; my_sweep_params.getAnglesForPosition(my_pan, my_tilt, k); calibrate_sweep_parameters.getAnglesForPosition(their_pan, their_tilt, corresp_pos); +printf("%s::%i\n",__FILE__,__LINE__); if ((my_pan != their_pan) ||(my_tilt != their_tilt)){ cerr<<"Error while finding corresponding registered transform. Sweep parameters: "< Date: Fri, 4 Nov 2016 16:12:08 +0100 Subject: [PATCH 183/255] debugg --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 44 +------------------ .../metaroom_additional_view_processing.cpp | 8 ---- 2 files changed, 2 insertions(+), 50 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index efa02505..0998d06f 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -236,7 +236,7 @@ std::vector getRegisteredViewPoses(const std::string& poses_fil Eigen::Matrix4d getMat(tf::StampedTransform tf){ - printf("getMat\n"); + //printf("getMat\n"); //Transform geometry_msgs::TransformStamped tfstmsg; tf::transformStampedTFToMsg (tf, tfstmsg); @@ -249,7 +249,7 @@ Eigen::Matrix4d getMat(tf::StampedTransform tf){ Eigen::Affine3d epose; tf::poseMsgToEigen(pose, epose); - std::cout << epose.matrix() << std::endl << std::endl; + //std::cout << epose.matrix() << std::endl << std::endl; return epose.matrix(); } @@ -264,46 +264,6 @@ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath) dummy.push_back("IntermediatePosition"); SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml",dummy); - - - -// printf("roomData.vIntermediateRoomCloudTransforms.size(): %i\n",roomData.vIntermediateRoomCloudTransforms.size()); -// for (size_t i=0; ifx = aCameraModel.fx(); -// cam->fy = aCameraModel.fy(); -// cam->cx = aCameraModel.cx(); -// cam->cy = aCameraModel.cy(); -// cam->print(); -// delete cam; -// } - - -// printf("roomData.vIntermediateRoomCloudCamParams.size(): %i\n",roomData.vIntermediateRoomCloudCamParams.size()); -// for (size_t i=0; ifx = aCameraModel.fx(); -// cam->fy = aCameraModel.fy(); -// cam->cx = aCameraModel.cx(); -// cam->cy = aCameraModel.cy(); -// cam->print(); -// delete cam; -// } -//exit(0); - reglib::Model * sweepmodel = 0; Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); std::vector current_room_frames; diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index fd9321da..063bdf48 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1823,14 +1823,6 @@ int main(int argc, char** argv){ if(roomObservationSubs.size() == 0){ roomObservationSubs.push_back(n.subscribe("/local_metric_map/room_observations", 1000, roomObservationCallback)); } - - // if(m_DynamicObjectsServiceServers.size() == 0){ - // m_DynamicObjectsServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/DynamicObjectsService", dynamicObjectsServiceCallback)); - // } - - // if(m_GetDynamicObjectServiceServers.size() == 0){ - // m_GetDynamicObjectServiceServers.push_back(n.advertiseService("/object_manager_node/ObjectManager/GetDynamicObjectService", getDynamicObjectServiceCallback)); - // } } printf("overall_folder: %s\n",overall_folder.c_str()); From 0354a384233580be2e87ffe94e71933ce1d64d83 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 4 Nov 2016 16:35:36 +0100 Subject: [PATCH 184/255] ... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 0998d06f..87005f20 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -263,12 +263,18 @@ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath) dummy.push_back("RoomIntermediateCloud"); dummy.push_back("IntermediatePosition"); SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml",dummy); - +printf("loaded room XML\n"); reglib::Model * sweepmodel = 0; Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); std::vector current_room_frames; - for (size_t i=0; i Date: Fri, 4 Nov 2016 16:44:05 +0100 Subject: [PATCH 185/255] ... --- .../quasimodo_brain/src/metaroom_additional_view_processing.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 063bdf48..62666506 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -152,6 +152,7 @@ void signal_callback_handler(int signum){ } void remove_old_seg(std::string sweep_folder){ + printf("remove_old_seg\n"); DIR *dir; struct dirent *ent; if ((dir = opendir (sweep_folder.c_str())) != NULL) { From a4ae313b52c577068b9716b01d1cd16ecfbcdf72 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 4 Nov 2016 16:50:57 +0100 Subject: [PATCH 186/255] ... --- .../src/metaroom_additional_view_processing.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 62666506..bab3910f 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -152,7 +152,7 @@ void signal_callback_handler(int signum){ } void remove_old_seg(std::string sweep_folder){ - printf("remove_old_seg\n"); + printf("remove_old_seg: %s\n",sweep_folder.c_str()); DIR *dir; struct dirent *ent; if ((dir = opendir (sweep_folder.c_str())) != NULL) { @@ -193,6 +193,8 @@ void remove_old_seg(std::string sweep_folder){ } closedir (dir); } + + printf("done remove_old_seg\n"); } void writePose(QXmlStreamWriter* xmlWriter, Eigen::Matrix4d pose){ @@ -1041,6 +1043,7 @@ printf("bgs.size() = %i\n",bgs.size()); else{ returnval = 3;} for(unsigned int i = 0; i < models.size(); i++){ + printf("processing model %i\n",i); std::vector internal_masks = internal[i]; std::vector external_masks = external[i]; std::vector dynamic_masks = dynamic[i]; From 5aa7bd8ae1ade690bfe1623441fa05555fa63a01 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 4 Nov 2016 20:54:36 +0100 Subject: [PATCH 187/255] ... --- .../src/metaroom_additional_view_processing.cpp | 4 +++- .../weightfunctions/GeneralizedGaussianDistribution.h | 8 ++++---- quasimodo/quasimodo_models/src/core/RGBDFrame.cpp | 5 ++++- .../weightfunctions/GeneralizedGaussianDistribution.cpp | 4 +++- 4 files changed, 14 insertions(+), 7 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index bab3910f..d46ee668 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1159,7 +1159,9 @@ printf("bgs.size() = %i\n",bgs.size()); std::stringstream ss_obj; ss_obj<m_label = ss_obj.str(); + std::string tmp = ss_obj.str(); + printf("ss_obj.str(): %s\n",tmp.c_str()); + roomObject->m_label = tmp; std::string xml_file = objectparser.saveAsXML(roomObject); printf("xml_file: %s\n",xml_file.c_str()); } diff --git a/quasimodo/quasimodo_models/include/weightfunctions/GeneralizedGaussianDistribution.h b/quasimodo/quasimodo_models/include/weightfunctions/GeneralizedGaussianDistribution.h index ea7d477f..245dcf97 100644 --- a/quasimodo/quasimodo_models/include/weightfunctions/GeneralizedGaussianDistribution.h +++ b/quasimodo/quasimodo_models/include/weightfunctions/GeneralizedGaussianDistribution.h @@ -42,11 +42,11 @@ class GeneralizedGaussianDistribution : public Distribution ~GeneralizedGaussianDistribution(); virtual void train(std::vector & hist, unsigned int nr_bins = 0); virtual void update(); - virtual double getInp(double x); + virtual double getInp(double x = 0); virtual double getNoise(); - virtual void setNoise(double x); - virtual double getval(double x); - virtual double getcdf(double x); + virtual void setNoise(double x = 0); + virtual double getval(double x = 0); + virtual double getcdf(double x = 0); virtual void print(); //virtual void getMaxdMind(double & maxd, double & mind, double prob = 0.1); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 7018cac5..3e70c1e8 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -774,7 +774,10 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt delete funcR; delete funcG; delete funcB; - + delete ggdfuncR; + delete ggdfuncG; + delete ggdfuncB; +exit(0); if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_prob.png", 127.0*re );} if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_prob.png", 127.0*ge );} diff --git a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp index b77c82e3..ac8bda1c 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp @@ -21,7 +21,9 @@ GeneralizedGaussianDistribution::GeneralizedGaussianDistribution(bool refine_std debugg_print = false; } -GeneralizedGaussianDistribution::~GeneralizedGaussianDistribution(){} +GeneralizedGaussianDistribution::~GeneralizedGaussianDistribution(){ + numcdf_vec.clear(); +} double GeneralizedGaussianDistribution::fitMul3(double mul, double mean, double std_mid, double power, std::vector & X, std::vector & Y, unsigned int nr_data, double costpen){ int iter = 25; From 9f4d1557f7338a8a825d04be23f75c53d7b97839 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 4 Nov 2016 21:21:24 +0100 Subject: [PATCH 188/255] ... --- quasimodo/quasimodo_models/src/core/RGBDFrame.cpp | 6 +++--- .../src/weightfunctions/DistanceWeightFunction2PPR3.cpp | 2 +- .../src/weightfunctions/GeneralizedGaussianDistribution.cpp | 1 + 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 3e70c1e8..7246ad3f 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -771,9 +771,9 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } - delete funcR; - delete funcG; - delete funcB; +// delete funcR; +// delete funcG; +// delete funcB; delete ggdfuncR; delete ggdfuncG; delete ggdfuncB; diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp index 90da4362..dc8d73ad 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp @@ -82,7 +82,7 @@ DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3(Distribution * dist_, d dist = dist_;//new GeneralizedGaussianDistribution(true,true);//GaussianDistribution();//GeneralizedGaussianDistribution();//GaussianDistribution(); } -DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3( double maxd_, int histogram_size_){ +DistanceWeightFunction2PPR3::DistancfuncZeWeightFunction2PPR3( double maxd_, int histogram_size_){ fixed_histogram_size = false; diff --git a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp index ac8bda1c..6f4703bb 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp @@ -19,6 +19,7 @@ GeneralizedGaussianDistribution::GeneralizedGaussianDistribution(bool refine_std power = power_; update(); debugg_print = false; + regularization = 0; } GeneralizedGaussianDistribution::~GeneralizedGaussianDistribution(){ From e5bef318b87a50ddd01ab1363bf19362c63f5f8b Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 4 Nov 2016 21:26:35 +0100 Subject: [PATCH 189/255] ... --- .../src/weightfunctions/DistanceWeightFunction2PPR3.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp index dc8d73ad..90da4362 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR3.cpp @@ -82,7 +82,7 @@ DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3(Distribution * dist_, d dist = dist_;//new GeneralizedGaussianDistribution(true,true);//GaussianDistribution();//GeneralizedGaussianDistribution();//GaussianDistribution(); } -DistanceWeightFunction2PPR3::DistancfuncZeWeightFunction2PPR3( double maxd_, int histogram_size_){ +DistanceWeightFunction2PPR3::DistanceWeightFunction2PPR3( double maxd_, int histogram_size_){ fixed_histogram_size = false; From 40148da02d7a6e7ebbe0c181236d31322981f3dc Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 4 Nov 2016 21:45:36 +0100 Subject: [PATCH 190/255] ... --- quasimodo/quasimodo_models/src/core/RGBDFrame.cpp | 12 ++++++------ .../GeneralizedGaussianDistribution.cpp | 5 +++-- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 7246ad3f..9f93a9e7 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -771,12 +771,12 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } -// delete funcR; -// delete funcG; -// delete funcB; - delete ggdfuncR; - delete ggdfuncG; - delete ggdfuncB; + delete funcR; + delete funcG; + delete funcB; +// delete ggdfuncR; +// delete ggdfuncG; +// delete ggdfuncB; exit(0); if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_prob.png", 127.0*re );} diff --git a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp index 6f4703bb..82a7ffe1 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/GeneralizedGaussianDistribution.cpp @@ -166,10 +166,11 @@ double GeneralizedGaussianDistribution::getcdf(double x){ double part = (x-start)/(stop-start); if(part < 0){return 0;} - if(part >= 1){return 1;} + // if(part >= 1){return 1;} part *= double(numcdf_vec.size()-1); unsigned int id0 = part; unsigned int id1 = id0+1; + if(id1 >= numcdf_vec.size()){return 1;} double w0 = double(id1)-part; double w1 = part-double(id0); double cdfx = numcdf_vec[id0]*w0 + numcdf_vec[id1]*w1; @@ -197,7 +198,7 @@ void GeneralizedGaussianDistribution::update_numcdf_vec(unsigned int bins, doubl for(unsigned int i = 0; i < bins; i++){ double x = start+double(i)*step+0.5*step; numcdf_vec[i] = sum; - sum += getval(x); + if(i < bins-1){sum += getval(x);} } for(unsigned int i = 0; i < bins; i++){numcdf_vec[i] /= sum;} } From c66979e7850906005eaf25fc4c53d85b0db271d7 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 4 Nov 2016 21:50:08 +0100 Subject: [PATCH 191/255] ... --- quasimodo/quasimodo_models/src/core/RGBDFrame.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 9f93a9e7..36010356 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -777,7 +777,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt // delete ggdfuncR; // delete ggdfuncG; // delete ggdfuncB; -exit(0); +//exit(0); if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_prob.png", 127.0*re );} if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_prob.png", 127.0*ge );} @@ -996,7 +996,8 @@ pcl::PointCloud::Ptr RGBDFrame::getPCLcloud(){ cloud->height = height; cloud->points.resize(width*height); - for(unsigned int w = 0; w < width; w++){ + for(unsigned int w = 0; w Date: Fri, 4 Nov 2016 21:52:18 +0100 Subject: [PATCH 192/255] ... --- quasimodo/quasimodo_models/src/core/RGBDFrame.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 36010356..320d9819 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -996,8 +996,7 @@ pcl::PointCloud::Ptr RGBDFrame::getPCLcloud(){ cloud->height = height; cloud->points.resize(width*height); - for(unsigned int w = 0; w Date: Fri, 4 Nov 2016 21:57:03 +0100 Subject: [PATCH 193/255] ... --- .../src/metaroom_additional_view_processing.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index d46ee668..1017a9b5 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1161,7 +1161,8 @@ printf("bgs.size() = %i\n",bgs.size()); ss_obj<<"_object_";ss_obj<<(dynamicCounter-1); std::string tmp = ss_obj.str(); printf("ss_obj.str(): %s\n",tmp.c_str()); - roomObject->m_label = tmp; + //roomObject->m_label = tmp; + roomObject->setLabel(tmp); std::string xml_file = objectparser.saveAsXML(roomObject); printf("xml_file: %s\n",xml_file.c_str()); } From af2d1fbdacf1affe35bc6d003bcd47eec2805901 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 4 Nov 2016 21:57:16 +0100 Subject: [PATCH 194/255] ... --- .../include/object_manager/dynamic_object.h | 3 ++- object_manager/src/dynamic_object.cpp | 6 ++++++ semantic_map/src/reg_transforms.cpp | 18 +++++++++--------- 3 files changed, 17 insertions(+), 10 deletions(-) diff --git a/object_manager/include/object_manager/dynamic_object.h b/object_manager/include/object_manager/dynamic_object.h index c0a1a9f2..a8c396e7 100644 --- a/object_manager/include/object_manager/dynamic_object.h +++ b/object_manager/include/object_manager/dynamic_object.h @@ -44,11 +44,12 @@ class DynamicObject { } }; - DynamicObject(bool verbose = false); + DynamicObject(bool verbose = false); ~DynamicObject(); void setTime(boost::posix_time::ptime); void setCloud(CloudPtr); + void setLabel(std::string label_); std::vector getAdditionalViewTransforms(); void addAdditionalViewTransform(tf::StampedTransform); diff --git a/object_manager/src/dynamic_object.cpp b/object_manager/src/dynamic_object.cpp index e01f1356..49b3c52f 100644 --- a/object_manager/src/dynamic_object.cpp +++ b/object_manager/src/dynamic_object.cpp @@ -20,13 +20,19 @@ DynamicObject::DynamicObject(bool verbose) : m_points(new Cloud()), m_normals(ne m_roomStringId = ""; m_noAdditionalViews = 0; m_AdditionalViewsTransformToObservation.setIdentity(); + //m_label = ""; } + + DynamicObject::~DynamicObject() { } + +void DynamicObject::setLabel(std::string label){m_label = label;} + void DynamicObject::setTime(boost::posix_time::ptime time) { m_time = time; diff --git a/semantic_map/src/reg_transforms.cpp b/semantic_map/src/reg_transforms.cpp index 87b35df6..8bdcca3e 100644 --- a/semantic_map/src/reg_transforms.cpp +++ b/semantic_map/src/reg_transforms.cpp @@ -372,23 +372,23 @@ std::vector semantic_map_registration_transforms::loadCorr std::string sweep_params_file, bool verbose) { -printf("%s::%i\n",__FILE__,__LINE__); +if(false){printf("%s::%i\n",__FILE__,__LINE__);} // load calibrated data std::vector allTransforms = semantic_map_registration_transforms::loadRegistrationTransforms(transforms_file, verbose); -printf("%s::%i\n",__FILE__,__LINE__); +if(false){printf("%s::%i\n",__FILE__,__LINE__);} std::vector empty; std::vector toRet; SweepParameters calibrate_sweep_parameters; -printf("%s::%i\n",__FILE__,__LINE__); +if(false){printf("%s::%i\n",__FILE__,__LINE__);} semantic_map_registration_transforms::loadSweepParameters(calibrate_sweep_parameters, verbose, sweep_params_file); -printf("%s::%i\n",__FILE__,__LINE__); +if(false){printf("%s::%i\n",__FILE__,__LINE__);} // find matching transforms for current sweep parameters from calibrate sweep parameters int number_of_positions = my_sweep_params.getNumberOfIntermediatePositions(); for (int k=0; k Date: Mon, 7 Nov 2016 12:49:44 +0100 Subject: [PATCH 195/255] Added the quasimodo conversions package --- .../quasimodo_conversions/CMakeLists.txt | 183 ++++++++++++++++++ .../quasimodo_conversions/conversions.h | 20 ++ quasimodo/quasimodo_conversions/package.xml | 54 ++++++ .../src/quasimodo_conversions/conversions.cpp | 136 +++++++++++++ 4 files changed, 393 insertions(+) create mode 100644 quasimodo/quasimodo_conversions/CMakeLists.txt create mode 100644 quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h create mode 100644 quasimodo/quasimodo_conversions/package.xml create mode 100644 quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp diff --git a/quasimodo/quasimodo_conversions/CMakeLists.txt b/quasimodo/quasimodo_conversions/CMakeLists.txt new file mode 100644 index 00000000..e8e28d43 --- /dev/null +++ b/quasimodo/quasimodo_conversions/CMakeLists.txt @@ -0,0 +1,183 @@ +cmake_minimum_required(VERSION 2.8.3) +project(quasimodo_conversions) + +add_definitions(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS roscpp quasimodo_msgs soma_llsd_msgs mongodb_store) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS include + LIBRARIES conversions + CATKIN_DEPENDS quasimodo_msgs soma_llsd_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories(include ${catkin_INCLUDE_DIRS}) + +## Declare a C++ library +add_library(conversions + src/${PROJECT_NAME}/conversions.cpp include/${PROJECT_NAME}/conversions.h +) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +add_dependencies(conversions ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(quasimodo_conversions_node src/quasimodo_conversions_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(quasimodo_conversions_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(conversions + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS quasimodo_conversions quasimodo_conversions_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_quasimodo_conversions.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h new file mode 100644 index 00000000..ba480705 --- /dev/null +++ b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h @@ -0,0 +1,20 @@ +#ifndef QUASIMODO_CONVERSIONS_H +#define QUASIMODO_CONVERSIONS_H + +#include +#include +#include +#include +#include +#include + +namespace quasimodo_conversions { + +void model_to_soma_segment(ros::NodeHandle& n, const quasimodo_msgs::model& model, soma_llsd_msgs::Segment& segment); +void soma_segment_to_model(ros::NodeHandle& n, const soma_llsd_msgs::Segment& segment, quasimodo_msgs::model& model); +void soma_observation_to_frame(ros::NodeHandle& n, const soma_llsd_msgs::Observation& obs, quasimodo_msgs::rgbd_frame& frame); +void frame_to_soma_observation(ros::NodeHandle& n, const quasimodo_msgs::rgbd_frame& frame, soma_llsd_msgs::Observation& obs); + +} // namespace quasimodo_conversions + +#endif // QUASIMODO_CONVERSIONS_H diff --git a/quasimodo/quasimodo_conversions/package.xml b/quasimodo/quasimodo_conversions/package.xml new file mode 100644 index 00000000..afd9c82b --- /dev/null +++ b/quasimodo/quasimodo_conversions/package.xml @@ -0,0 +1,54 @@ + + + quasimodo_conversions + 0.0.0 + The quasimodo_conversions package + + + + + nbore + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + quasimodo_msgs + soma_llsd_msgs + quasimodo_msgs + soma_llsd_msgs + + + + + + + + diff --git a/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp new file mode 100644 index 00000000..316979af --- /dev/null +++ b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp @@ -0,0 +1,136 @@ +#include +#include +#include + +namespace quasimodo_conversions { + +void model_to_soma_segment(ros::NodeHandle& n, const quasimodo_msgs::model& model, soma_llsd_msgs::Segment& segment) +{ + ros::ServiceClient client = n.serviceClient("/soma_llsd/insert_scene"); + ROS_INFO("Waiting for /soma_llsd/insert_scene service..."); + if (!client.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/insert_scene service!"); + return; + } + ROS_INFO("Got /soma_llsd/insert_scene service"); + + segment.id = std::to_string(model.model_id); + + size_t counter = 0; + for (const quasimodo_msgs::rgbd_frame& frame : model.frames) { + soma_llsd::InsertScene scene; + scene.request.rgb_img = frame.rgb; + scene.request.depth_img = frame.depth; + scene.request.camera_info = frame.camera; + scene.request.robot_pose = frame.pose; // not good actually but whatevs + scene.request.cloud = model.clouds[counter]; + + if (!client.call(scene)) { + ROS_ERROR("Failed to call service /soma_llsd/insert_scene"); + return; + } + + soma_llsd_msgs::Observation obs; + obs.scene_id = scene.response.response.id; + obs.camera_cloud = model.clouds[counter]; + obs.image_mask = model.masks[counter]; + //frame.camera = // see above + //frame.frame_id = // see + obs.pose = model.local_poses[counter]; //frame.pose; + //obs.pose = model.local_poses[counter]; + obs.id = std::to_string(frame.frame_id); + obs.timestamp = ros::Time::now().nsec; + segment.observations.push_back(obs); + ++counter; + } + +} + +void soma_segment_to_model(ros::NodeHandle& n, const soma_llsd_msgs::Segment& segment, quasimodo_msgs::model& model) +{ + ros::ServiceClient client = n.serviceClient("/soma_llsd/get_scene"); + ROS_INFO("Waiting for /soma_llsd/get_scene service..."); + if (!client.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/get_scene service!"); + return; + } + ROS_INFO("Got /soma_llsd/get_scene service"); + + // segment.scene_id; // this can be used to fetch the scene from mongodb, from which we can get the camera parameters + + model.model_id = std::stoi(segment.id); + + for (const soma_llsd_msgs::Observation& obs : segment.observations) { + soma_llsd::GetScene srv; + srv.request.scene_id = obs.scene_id; + if (!client.call(srv)) { + ROS_ERROR("Failed to call service /soma_llsd/get_scene"); + return; + } + model.clouds.push_back(obs.camera_cloud); + quasimodo_msgs::rgbd_frame frame; + frame.depth = srv.response.response.depth_img; + frame.rgb = srv.response.response.rgb_img; + model.masks.push_back(obs.image_mask); + //frame.camera = // see above + frame.frame_id = std::stoi(obs.id); + //frame.pose = obs.pose; + model.local_poses.push_back(obs.pose); + model.frames.push_back(frame); + model.local_poses.push_back(obs.pose); + //model.global_pose = // could probably get this from scene also? + //model.masks + } +} + +void frame_to_soma_observation(ros::NodeHandle& n, const quasimodo_msgs::rgbd_frame& frame, soma_llsd_msgs::Observation& obs) +{ + ros::ServiceClient client = n.serviceClient("/soma_llsd/insert_scene"); + ROS_INFO("Waiting for /soma_llsd/insert_scene service..."); + if (!client.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/insert_scene service!"); + return; + } + ROS_INFO("Got /soma_llsd/insert_scene service"); + + soma_llsd::InsertScene scene; + scene.request.rgb_img = frame.rgb; + scene.request.depth_img = frame.depth; + scene.request.camera_info = frame.camera; + scene.request.robot_pose = frame.pose; // not good actually but whatevs + + if (!client.call(scene)) { + ROS_ERROR("Failed to call service /soma_llsd/insert_scene"); + return; + } + + obs.scene_id = scene.response.response.id; + obs.id = std::to_string(frame.frame_id); + obs.pose = frame.pose; +} + +void soma_observation_to_frame(ros::NodeHandle& n, const soma_llsd_msgs::Observation& obs, quasimodo_msgs::rgbd_frame& frame) +{ + ros::ServiceClient client = n.serviceClient("/soma_llsd/get_scene"); + ROS_INFO("Waiting for /soma_llsd/get_scene service..."); + if (!client.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/get_scene service!"); + return; + } + ROS_INFO("Got /soma_llsd/get_scene service"); + + soma_llsd::GetScene srv; + srv.request.scene_id = obs.scene_id; + if (!client.call(srv)) { + ROS_ERROR("Failed to call service /soma_llsd/get_scene"); + return; + } + + frame.depth = srv.response.response.depth_img; + frame.rgb = srv.response.response.rgb_img; + //frame.camera = // see above + frame.frame_id = std::stoi(obs.id); + frame.pose = obs.pose; // not good actually but whatevs +} + +} // namespace quasimodo_conversions From ed0a6f51fccd124d519dc34be8ea5eb9ca9ad3eb Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 7 Nov 2016 14:14:48 +0100 Subject: [PATCH 196/255] Added catkin ignore to conversions package until soma_llsd is part of the distribution --- quasimodo/quasimodo_conversions/CATKIN_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 quasimodo/quasimodo_conversions/CATKIN_IGNORE diff --git a/quasimodo/quasimodo_conversions/CATKIN_IGNORE b/quasimodo/quasimodo_conversions/CATKIN_IGNORE new file mode 100644 index 00000000..e69de29b From 1b1bd393a7ef18bc92a50a3c787ceb4cb506890c Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 7 Nov 2016 14:24:18 +0100 Subject: [PATCH 197/255] Disabled building of the stopwatch project when compiling with catkiN --- .../dynamic_object_retrieval/CMakeLists.txt | 27 ++++++++++--------- 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/CMakeLists.txt b/dynamic_object_retrieval/dynamic_object_retrieval/CMakeLists.txt index 7516f794..53d84523 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/CMakeLists.txt +++ b/dynamic_object_retrieval/dynamic_object_retrieval/CMakeLists.txt @@ -16,6 +16,9 @@ add_definitions(-std=c++11 -O3) # Show where to find the find package scripts list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake") +set(stopwatch_dir ${CMAKE_CURRENT_SOURCE_DIR}/stopwatch) +set(stopwatch_include_dir ${stopwatch_dir}/src) + # Define the locations of the k_means_tree project # If using catkin, including it using catkin instead if (catkin_FOUND) @@ -23,6 +26,7 @@ if (catkin_FOUND) metaroom_xml_parser object_manager k_means_tree convex_segmentation) set(ROS_LIBRARIES ${catkin_LIBRARIES}) include_directories(${catkin_INCLUDE_DIRS}) + #set(stopwatch_library_dir ${CATKIN_DEVEL_PREFIX}/stopwatch/build) else() set(k_means_tree_dir ${CMAKE_CURRENT_SOURCE_DIR}/../k_means_tree) set(k_means_tree_library_dir ${k_means_tree_dir}/build) @@ -57,24 +61,23 @@ else() # Define the locations of cereal serialization header files set(cereal_include_dir ${CMAKE_CURRENT_SOURCE_DIR}/../k_means_tree/cereal/include) + + # Only compile the stopwatch project if it's not with catkin include_directories(${cereal_include_dir}) + set(stopwatch_library_dir ${stopwatch_dir}/build) + # Compile the stopwatch project, needed for this + ExternalProject_Add(stopwatch_project + DOWNLOAD_COMMAND "" + SOURCE_DIR ${stopwatch_include_dir} + BINARY_DIR ${stopwatch_library_dir} + BUILD_COMMAND ${CMAKE_MAKE_PROGRAM} + INSTALL_COMMAND "" + ) endif() # Eh, we need to somehow build this maybe? -set(stopwatch_dir ${CMAKE_CURRENT_SOURCE_DIR}/stopwatch) -set(stopwatch_include_dir ${stopwatch_dir}/src) -set(stopwatch_library_dir ${stopwatch_dir}/build) include_directories(${stopwatch_include_dir}) -# Compile the stopwatch project, needed for this -ExternalProject_Add(stopwatch_project - DOWNLOAD_COMMAND "" - SOURCE_DIR ${stopwatch_include_dir} - BINARY_DIR ${stopwatch_library_dir} - BUILD_COMMAND ${CMAKE_MAKE_PROGRAM} - INSTALL_COMMAND "" -) - # Find PCL find_package(PCL 1.6 REQUIRED COMPONENTS common io search visualization surface kdtree features surface segmentation octree filter keypoints) include_directories(${PCL_INCLUDE_DIRS}) From 515a44631421ef0d3c56dcdebd7386ba87ce52af Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 7 Nov 2016 14:25:10 +0100 Subject: [PATCH 198/255] Added json serialization for eigen --- .../include/eigen_cereal/eigen_cereal.h | 48 ++++++++++++++++++- 1 file changed, 47 insertions(+), 1 deletion(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/eigen_cereal/eigen_cereal.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/eigen_cereal/eigen_cereal.h index c00f039b..8330b6ab 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/eigen_cereal/eigen_cereal.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/eigen_cereal/eigen_cereal.h @@ -3,11 +3,14 @@ #include #include -#include +#include +#include #include namespace cereal { + +// for binary serialization template inline typename std::enable_if, Archive>::value, void>::type save(Archive & ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> const & m) @@ -30,6 +33,49 @@ load(Archive & ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _Max m.resize(rows, cols); ar(binary_data(m.data(), static_cast(rows * cols * sizeof(_Scalar)))); } + +// for json serialization +template inline +typename std::enable_if, Archive>::value, void>::type +save(Archive & ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> const & m) +{ + int32_t rows = m.rows(); + int32_t cols = m.cols(); + ar(rows); + ar(cols); + + std::vector > vec(rows); + for (int i = 0; i < rows; i++) { + vec[i].resize(cols); + for (int j = 0; j < cols; j++) { + vec[i][j] = m(i, j); + } + } + + ar(vec); +} + +template inline +typename std::enable_if, Archive>::value, void>::type +load(Archive & ar, Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> & m) +{ + int32_t rows; + int32_t cols; + ar(rows); + ar(cols); + std::vector > vec; + ar(vec); + + m.resize(rows, cols); + + for (int i = 0; i < rows; i++) { + for (int j = 0; j < cols; j++) { + m(i, j) = vec[i][j]; + } + } + +} + } #endif // EIGEN_CEREAL_H From f89cc5f3c1ef8f2645aa311b0920303934c0cce7 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 7 Nov 2016 14:25:41 +0100 Subject: [PATCH 199/255] Fixed install bug in quasimodo models --- quasimodo/quasimodo_models/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/quasimodo/quasimodo_models/CMakeLists.txt b/quasimodo/quasimodo_models/CMakeLists.txt index 3d16a0b5..6f2617f0 100644 --- a/quasimodo/quasimodo_models/CMakeLists.txt +++ b/quasimodo/quasimodo_models/CMakeLists.txt @@ -95,6 +95,3 @@ install(TARGETS quasimodo_weightlib quasimodo_core quasimodo_reglib quasimodo_ma RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) From a5d49431df91df16978708f5dda468aa7d063d4e Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 7 Nov 2016 14:32:42 +0100 Subject: [PATCH 200/255] Added install target for quasimodo object search --- quasimodo/quasimodo_object_search/CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/quasimodo/quasimodo_object_search/CMakeLists.txt b/quasimodo/quasimodo_object_search/CMakeLists.txt index 1a70e6bb..b0343ef2 100644 --- a/quasimodo/quasimodo_object_search/CMakeLists.txt +++ b/quasimodo/quasimodo_object_search/CMakeLists.txt @@ -146,6 +146,12 @@ catkin_package( # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +install(DIRECTORY scripts/ + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + USE_SOURCE_PERMISSIONS) + ## Mark executables and/or libraries for installation # install(TARGETS quasimodo_object_search2 quasimodo_object_search2_node # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} From 7cee254391dc2c52ec04e4b14b62df3c2cd50f4a Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 7 Nov 2016 14:55:03 +0100 Subject: [PATCH 201/255] Changed from soma2 to soma --- quasimodo/quasimodo_brain/CMakeLists.txt | 2 +- quasimodo/quasimodo_brain/package.xml | 4 +-- quasimodo/quasimodo_brain/src/massregPCD.cpp | 4 +-- quasimodo/quasimodo_brain/src/modelserver.cpp | 28 +++++++++---------- .../create_object_search_observation.py | 8 +++--- .../scripts/insert_object_server.py | 2 +- .../scripts/retrieve_object_search.py | 2 +- quasimodo/quasimodo_retrieval/CMakeLists.txt | 4 +-- quasimodo/quasimodo_retrieval/package.xml | 4 +-- .../src/quasimodo_logging_server.cpp | 12 ++++---- 10 files changed, 35 insertions(+), 35 deletions(-) diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index b5d2e4dd..703bd6ed 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.3) project(quasimodo_brain) -find_package(catkin REQUIRED COMPONENTS roscpp soma2_msgs soma_manager quasimodo_msgs quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge) +find_package(catkin REQUIRED COMPONENTS roscpp soma_msgs soma_manager quasimodo_msgs quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge) set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fPIC -std=c++0x -o popcnt -mssse3") diff --git a/quasimodo/quasimodo_brain/package.xml b/quasimodo/quasimodo_brain/package.xml index 0f0281f1..2e796cd9 100644 --- a/quasimodo/quasimodo_brain/package.xml +++ b/quasimodo/quasimodo_brain/package.xml @@ -51,7 +51,7 @@ libqt4-dev libceres-dev roscpp - soma2_msgs + soma_msgs soma_manager message_runtime eigen_conversions @@ -66,7 +66,7 @@ libqt4 libceres-dev roscpp - soma2_msgs + soma_msgs soma_manager message_runtime eigen_conversions diff --git a/quasimodo/quasimodo_brain/src/massregPCD.cpp b/quasimodo/quasimodo_brain/src/massregPCD.cpp index 71fe4d34..496ee58a 100644 --- a/quasimodo/quasimodo_brain/src/massregPCD.cpp +++ b/quasimodo/quasimodo_brain/src/massregPCD.cpp @@ -15,9 +15,9 @@ #include "quasimodo_msgs/retrieval_query_result.h" #include "quasimodo_msgs/retrieval_query.h" #include "quasimodo_msgs/retrieval_result.h" -#include "soma2_msgs/SOMA2Object.h" +#include "soma_msgs/SOMAObject.h" -#include "soma_manager/SOMA2InsertObjs.h" +#include "soma_manager/SOMAInsertObjs.h" //#include "modelupdater/ModelUpdater.h" //#include "/home/johane/catkin_ws_dyn/src/quasimodo_models/include/modelupdater/ModelUpdater.h" diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 10b95d35..e015cb50 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -15,9 +15,9 @@ #include "quasimodo_msgs/retrieval_query_result.h" #include "quasimodo_msgs/retrieval_query.h" #include "quasimodo_msgs/retrieval_result.h" -#include "soma2_msgs/SOMA2Object.h" +#include "soma_msgs/SOMAObject.h" -#include "soma_manager/SOMA2InsertObjs.h" +#include "soma_manager/SOMAInsertObjs.h" //#include "modelupdater/ModelUpdater.h" //#include "/home/johane/catkin_ws_dyn/src/quasimodo_models/include/modelupdater/ModelUpdater.h" @@ -73,7 +73,7 @@ ros::Publisher model_pcd_pub; ros::Publisher database_pcd_pub; ros::Publisher chatter_pub; -ros::ServiceClient soma2add; +ros::ServiceClient soma_add; double occlusion_penalty = 15; double massreg_timeout = 60; @@ -247,10 +247,10 @@ quasimodo_msgs::model getModelMSG(reglib::Model * model){ return msg; } -std::vector getSOMA2ObjectMSGs(reglib::Model * model){ - std::vector msgs; +std::vector getSOMA2ObjectMSGs(reglib::Model * model){ + std::vector msgs; for(unsigned int i = 0; i < model->frames.size(); i++){ - soma2_msgs::SOMA2Object msg; + soma_msgs::SOMAObject msg; char buf [1024]; sprintf(buf,"id_%i_%i",int(model->id),int(model->frames[i]->id)); msg.id = std::string(buf); @@ -346,17 +346,17 @@ std::vector getSOMA2ObjectMSGs(reglib::Model * model){ } void publishModel(reglib::Model * model){ - // std::vector somamsgs = getSOMA2ObjectMSGs(model); - soma_manager::SOMA2InsertObjs objs; + // std::vector somamsgs = getSOMA2ObjectMSGs(model); + soma_manager::SOMAInsertObjs objs; objs.request.objects = getSOMA2ObjectMSGs(model); - if (soma2add.call(objs)){//Add frame to model server + if (soma_add.call(objs)){//Add frame to model server //// int frame_id = ifsrv.response.frame_id; - }else{ROS_ERROR("Failed to call service soma2add");} + }else{ROS_ERROR("Failed to call service soma_add");} // for(int i = 0; i < somamsgs.size(); i++){ - // if (soma2add.call(somamsgs[i])){//Add frame to model server + // if (soma_add.call(somamsgs[i])){//Add frame to model server //// int frame_id = ifsrv.response.frame_id; - // }else{ROS_ERROR("Failed to call service soma2add");} + // }else{ROS_ERROR("Failed to call service soma_add");} // } } @@ -902,8 +902,8 @@ int main(int argc, char **argv){ ros::ServiceServer service4 = n.advertiseService("get_model", getModel); ROS_INFO("Ready to add use get_model."); - soma2add = n.serviceClient( "soma2/insert_objs"); - ROS_INFO("Ready to add use soma2add."); + soma_add = n.serviceClient("soma/insert_objs"); + ROS_INFO("Ready to add use soma_add."); ros::Subscriber sub = n.subscribe("/retrieval_result", 1, retrievalCallback); ROS_INFO("Ready to add recieve search results."); diff --git a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py index 4dbd0d73..1b2e5fca 100755 --- a/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py +++ b/quasimodo/quasimodo_object_search/scripts/create_object_search_observation.py @@ -8,7 +8,7 @@ import cv2 # SOMA2 stuff -from soma2_msgs.msg import SOMA2Object +from soma_msgs.msg import SOMAObject from mongodb_store.message_store import MessageStoreProxy from soma_manager.srv import * from geometry_msgs.msg import Pose, Transform @@ -27,11 +27,11 @@ def chron_callback(): print("making query") - soma_query = rospy.ServiceProxy('soma2/query_db',SOMA2QueryObjs) + soma_query = rospy.ServiceProxy('soma/query_db',SOMAQueryObjs) print("done query") # Query all observations during the last 30 mins - query = SOMA2QueryObjsRequest() + query = SOMAQueryObjsRequest() query.query_type = 0 query.objecttypes=['unknown'] query.uselowertime = True @@ -166,7 +166,7 @@ def chron_callback(): if __name__ == '__main__': rospy.init_node('create_object_search_observation', anonymous = False) print("getting soma service") - rospy.wait_for_service('soma2/query_db') + rospy.wait_for_service('soma/query_db') print("done initializing") r = rospy.Rate(1.0/(60.0*UPDATE_INT_MINUTES)) # 10hz diff --git a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py index 85f5a5ab..73f34db0 100755 --- a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py +++ b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py @@ -8,7 +8,7 @@ import cv2 # SOMA2 stuff -from soma2_msgs.msg import SOMA2Object +from soma_msgs.msg import SOMAObject from mongodb_store.message_store import MessageStoreProxy from soma_manager.srv import * from geometry_msgs.msg import Pose, Transform diff --git a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py index 751eb5eb..eaeda48f 100755 --- a/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py +++ b/quasimodo/quasimodo_object_search/scripts/retrieve_object_search.py @@ -9,7 +9,7 @@ import numpy as np # SOMA2 stuff -from soma2_msgs.msg import SOMA2Object +from soma_msgs.msg import SOMAObject from mongodb_store.message_store import MessageStoreProxy from soma_manager.srv import * from geometry_msgs.msg import Pose, Transform diff --git a/quasimodo/quasimodo_retrieval/CMakeLists.txt b/quasimodo/quasimodo_retrieval/CMakeLists.txt index 71bc0070..484733bf 100644 --- a/quasimodo/quasimodo_retrieval/CMakeLists.txt +++ b/quasimodo/quasimodo_retrieval/CMakeLists.txt @@ -8,7 +8,7 @@ add_definitions(-std=c++11 -O3) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp tf sensor_msgs std_msgs image_geometry metaroom_xml_parser quasimodo_msgs message_runtime message_generation tf_conversions pcl_ros image_geometry cv_bridge tf_conversions eigen_conversions - k_means_tree dynamic_object_retrieval object_3d_benchmark dynamic_reconfigure soma2_msgs) + k_means_tree dynamic_object_retrieval object_3d_benchmark dynamic_reconfigure soma_msgs) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -186,7 +186,7 @@ add_dependencies(quasimodo_visualization_server quasimodo_msgs_generate_messages add_dependencies(quasimodo_visualize_model quasimodo_msgs_generate_messages_cpp) add_dependencies(quasimodo_retrieval_node quasimodo_msgs_generate_messages_cpp) add_dependencies(quasimodo_retrieve_observation quasimodo_msgs_generate_messages_cpp) -add_dependencies(quasimodo_logging_server quasimodo_msgs_generate_messages_cpp soma2_msgs_generate_messages_cpp) +add_dependencies(quasimodo_logging_server quasimodo_msgs_generate_messages_cpp soma_msgs_generate_messages_cpp) ## Add cmake target dependencies of the executable ## same as for the library above diff --git a/quasimodo/quasimodo_retrieval/package.xml b/quasimodo/quasimodo_retrieval/package.xml index 967233e0..9b5915f1 100644 --- a/quasimodo/quasimodo_retrieval/package.xml +++ b/quasimodo/quasimodo_retrieval/package.xml @@ -58,7 +58,7 @@ dynamic_reconfigure libqt4-dev libqt4-opengl-dev - soma2_msgs + soma_msgs libpcl-all libopencv-dev cv_bridge @@ -77,7 +77,7 @@ dynamic_reconfigure libqt4 libqt4-opengl - soma2_msgs + soma_msgs diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp index 8e72164b..54af76b7 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp @@ -14,8 +14,8 @@ #include #include -// for logging to soma2 in mongodb -#include +// for logging to soma in mongodb +#include #include #include @@ -70,7 +70,7 @@ class logging_server { soma_req.objects[i].id = res.result.retrieved_image_paths[i].strings[0]; } - std::sort(soma_req.objects.begin(), soma_req.objects.end(), [](const soma2_msgs::SOMA2Object& o1, const soma2_msgs::SOMA2Object& o2) { + std::sort(soma_req.objects.begin(), soma_req.objects.end(), [](const soma_msgs::SOMAObject& o1, const soma_msgs::SOMAObject& o2) { return o1.logtimestamp < o2.logtimestamp; }); @@ -85,8 +85,8 @@ class logging_server { soma_req.objects[N].id = "retrieval_query"; */ - ros::ServiceClient soma_service = n.serviceClient("/soma2/insert_objects"); - soma_manager::SOMA2InsertObjs::Response soma_resp; + ros::ServiceClient soma_service = n.serviceClient("/soma/insert_objects"); + soma_manager::SOMAInsertObjs::Response soma_resp; try { if (soma_service.call(soma_req, soma_resp)) { cout << "Successfully inserted queries!" << endl; @@ -103,7 +103,7 @@ class logging_server { mongodb_store::MongoInsert::Request db_req; //db_req. - ros::ServiceClient db_service = n.serviceClient("/soma2/insert_objects"); + ros::ServiceClient db_service = n.serviceClient("/soma/insert_objects"); mongodb_store::MongoInsert::Response db_resp; if (db_service.call(db_req, db_resp)) { cout << "Successfully inserted queries!" << endl; From 20adc85b3b3c4f1bd4e248da1f87fe2cd155eb09 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 7 Nov 2016 15:10:59 +0100 Subject: [PATCH 202/255] bugfixxes and such --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 4 +- quasimodo/quasimodo_brain/src/Util/Util.h | 2 +- .../metaroom_additional_view_processing.cpp | 63 ++-- quasimodo/quasimodo_brain/src/modelserver.cpp | 347 +----------------- .../quasimodo_models/include/core/RGBDFrame.h | 2 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 273 +++++--------- .../src/modelupdater/ModelUpdater.cpp | 2 +- 7 files changed, 155 insertions(+), 538 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 87005f20..2f29661d 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -115,7 +115,7 @@ double getTime(){ return double(start1.tv_sec+(start1.tv_usec/1000000.0)); } -reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg){ +reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg, bool compute_edges){ reglib::Model * model = new reglib::Model(); for(unsigned int i = 0; i < msg.local_poses.size(); i++){ @@ -144,7 +144,7 @@ reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg){ cam->cy = camera.K[5]; } - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix()); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, double(capture_time.sec)+double(capture_time.nsec)/1000000000.0, epose.matrix(),true,"",compute_edges); model->frames.push_back(frame); geometry_msgs::Pose pose1 = msg.local_poses[i]; diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index ebb4a7d0..cef25163 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -45,7 +45,7 @@ std::vector getRegisteredViewPosesFromFile(std::string poses_fi reglib::Model * loadFromRaresFormat(std::string path); double getTime(); -reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg); +reglib::Model * getModelFromMSG(quasimodo_msgs::model & msg, bool compute_edges = true); void addToModelMSG(quasimodo_msgs::model & msg, reglib::Model * model, Eigen::Affine3d rp = Eigen::Affine3d::Identity(), bool addClouds = false); quasimodo_msgs::model getModelMSG(reglib::Model * model, bool addClouds = false); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 1017a9b5..5118d55a 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -143,6 +143,7 @@ reglib::Camera * basecam; bool recomputeRelativePoses = false; +void sendMetaroomToServer(std::string path); bool testDynamicObjectServiceCallback(std::string path); bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res); @@ -383,7 +384,7 @@ int readNumberOfViews(std::string xmlFile){ } void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses, bool compute_edges = true, std::string savePath = ""){ - + //printf("readViewXML: compute_edges: %i\n",compute_edges); QFile file(xmlFile.c_str()); if (!file.exists()) @@ -501,7 +502,7 @@ void readViewXML(std::string xmlFile, std::vector & frames, token = xmlReader->readNext();//Pose elementName = xmlReader->name().toString(); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, regpose,true,savePath); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth, time, regpose,true,savePath,compute_edges); frames.push_back(frame); poses.push_back(pose); } @@ -938,7 +939,7 @@ int processMetaroom(std::string path, bool store_old_xml = true){ quasimodo_brain::cleanPath(path); int returnval = 0; - printf("processing: %s\n",path.c_str()); + printf("processMetaroom: %s\n",path.c_str()); if ( ! boost::filesystem::exists( path ) ){return 0;} @@ -982,7 +983,7 @@ int processMetaroom(std::string path, bool store_old_xml = true){ // std::cout << roomTransform << std::endl; // return 0; -//exit(0); + DynamicObjectXMLParser objectparser(sweep_folder, true); @@ -992,16 +993,23 @@ int processMetaroom(std::string path, bool store_old_xml = true){ int prevind = -1; std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + //printf("sweep_xmls: %i\n",sweep_xmls.size()); for (unsigned int i = 0; i < sweep_xmls.size(); i++){ SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); std::string other_waypointid = other_roomData.roomWaypointId; + //printf("%i: %s\n",i,sweep_xmls[i].c_str()); if(sweep_xmls[i].compare(path) == 0){break;} if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} } + printf("prevind: %i\n",prevind); + printf("current: %s\n",path.c_str()); + int nextind = sweep_xmls.size(); - for (unsigned int i = (sweep_xmls.size()-1); i >= 0 ; i--){ + for (int i = int(sweep_xmls.size()-1); i >= 0 ; i--){ + if(i < 0){break;} + //printf("%i / %i \n",i,sweep_xmls.size()); SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); std::string other_waypointid = other_roomData.roomWaypointId; @@ -1022,7 +1030,9 @@ int processMetaroom(std::string path, bool store_old_xml = true){ printf("prev: %s\n",prev.c_str()); reglib::Model * bg = getAVMetaroom(prev); bgs.push_back(bg); - } + }else{ + printf("no previous...\n"); + } // if(nextind < sweep_xmls.size()){ // std::string next = sweep_xmls[nextind]; @@ -1382,6 +1392,7 @@ printf("bgs.size() = %i\n",bgs.size()); for(unsigned int i = 0; i < out_pubs.size(); i++){out_pubs[i].publish(msg);} ros::spinOnce(); + //sendMetaroomToServer(path); return returnval; } @@ -1420,14 +1431,13 @@ void trainMetaroom(std::string path){ } std::vector loadModels(std::string path){ - printf("loadModels: %s\n",path.c_str()); std::vector models; int slash_pos = path.find_last_of("/"); std::string sweep_folder = path.substr(0, slash_pos) + "/"; std::vector frames; std::vector poses; - readViewXML(sweep_folder+"ViewGroup.xml",frames,poses); + readViewXML(sweep_folder+"ViewGroup.xml",frames,poses,false); QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml")); for (auto objectFile : objectFiles){ @@ -1442,7 +1452,7 @@ std::vector loadModels(std::string path){ } file.open(QIODevice::ReadOnly); - ROS_INFO_STREAM("Parsing xml file: "< loadModels(std::string path){ if (attributes.hasAttribute("image_number")){ QString depthpath = attributes.value("image_number").toString(); number = atoi(depthpath.toStdString().c_str()); - printf("number: %i\n",number); + //printf("number: %i\n",number); }else{break;} mod->frames.push_back(frames[number]->clone()); @@ -1490,7 +1500,6 @@ std::vector loadModels(std::string path){ } for(unsigned int i = 0; i < frames.size(); i++){delete frames[i];} - return models; } @@ -1501,11 +1510,12 @@ void addModelToModelServer(reglib::Model * model){ } void sendMetaroomToServer(std::string path){ - std::vector mods = loadModels(path); - for(unsigned int i = 0; i < mods.size(); i++){ - addModelToModelServer(mods[i]); - mods[i]->fullDelete(); - delete mods[i]; + std::vector models = loadModels(path); + for(unsigned int i = 0; i < models.size(); i++){ + printf("%i\n",i); + addModelToModelServer(models[i]); + models[i]->fullDelete(); + delete models[i]; } } @@ -1812,19 +1822,19 @@ int main(int argc, char** argv){ } if(segsubs.size() == 0){ - segsubs.push_back(n.subscribe("/some/inputtopic", 1000, chatterCallback)); + segsubs.push_back(n.subscribe("/quasimodo/segmentation/in", 1000, chatterCallback)); } - if(out_pubs.size() == 0){ - out_pubs.push_back(n.advertise("/some/topic", 1000)); + if(out_pubs.size() == 0){ + out_pubs.push_back(n.advertise("/quasimodo/segmentation/out/path", 1000)); } if(model_pubs.size() == 0){ - model_pubs.push_back(n.advertise("/model/out/topic", 1000)); + model_pubs.push_back(n.advertise("/quasimodo/segmentation/out/model", 1000)); } if(sendsubs.size() == 0){ - sendsubs.push_back(n.subscribe("/model/out/topic/path", 1000, sendCallback)); + sendsubs.push_back(n.subscribe("/quasimodo/segmentation/send", 1000, sendCallback)); } if(roomObservationSubs.size() == 0){ @@ -1837,7 +1847,16 @@ int main(int argc, char** argv){ for(unsigned int i = 0; i < raresfiles.size(); i++){ segmentRaresFiles( raresfiles[i], raresfiles_resegment[i]);} for(unsigned int i = 0; i < trainMetarooms.size(); i++){ trainMetaroom( trainMetarooms[i]);} for(unsigned int i = 0; i < processMetarooms.size(); i++){ processMetaroom( processMetarooms[i]);} - for(unsigned int i = 0; i < sendMetaroomToServers.size(); i++){ sendMetaroomToServer( sendMetaroomToServers[i]);} + for(unsigned int i = 0; i < sendMetaroomToServers.size(); i++){ + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(sendMetaroomToServers[i]); + for (auto sweep_xml : sweep_xmls) { + printf("sweep_xml: %s\n",sweep_xml.c_str()); + quasimodo_brain::cleanPath(sweep_xml); + int slash_pos = sweep_xml.find_last_of("/"); + std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; + sendMetaroomToServer(sweep_folder); + } + } if(!once){ros::spin();} printf("done...\n"); diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 7bab9cd9..bca99ac4 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -640,23 +640,28 @@ bool runSearch(ModelDatabase * database, reglib::Model * model, int number_of_se std::set searchset; void addNewModel(reglib::Model * model){ +printf("%s::%i\n",__FILE__,__LINE__); reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model, reg); mu->occlusion_penalty = occlusion_penalty; mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; +printf("%s::%i\n",__FILE__,__LINE__); mu->show_init_lvl = show_init_lvl;//init show mu->show_refine_lvl = show_refine_lvl;//refine show mu->show_scoring = show_scoring;//fuse scoring show reg->visualizationLvl = show_reg_lvl; +printf("%s::%i\n",__FILE__,__LINE__); mu->makeInitialSetup(); delete mu; delete reg; +printf("%s::%i\n",__FILE__,__LINE__); + reglib::Model * newmodelHolder = new reglib::Model(); newmodelHolder->submodels.push_back(model); newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); @@ -666,11 +671,12 @@ void addNewModel(reglib::Model * model){ }else{ newmodelHolder->recomputeModelPoints(); } - +printf("%s::%i\n",__FILE__,__LINE__); modeldatabase->add(newmodelHolder); addToDB(modeldatabase, newmodelHolder,false); show_sorted(); +printf("%s::%i\n",__FILE__,__LINE__); bool do_next = true; while(do_next && run_search){ printf("running search loop\n"); @@ -696,335 +702,13 @@ void addNewModel(reglib::Model * model){ publishDatabasePCD(); dumpDatabase(savepath); - - - - - // modeldatabase->add(newmodelHolder); - // addToDB(modeldatabase, newmodelHolder,false); - // for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - // publish_history(modeldatabase->models[i]->getHistory()); - // } - // /* - // for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - // publish_history(modeldatabase->models[i]->getHistory()); - // }*/ - // show_sorted(); - // publishDatabasePCD(); - // dumpDatabase(savepath); - - - // modeldatabase->add(model); - // addToDB(modeldatabase, model,false); - // for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - // publish_history(modeldatabase->models[i]->getHistory()); - // } - // publishDatabasePCD(); - // dumpDatabase(savepath); - - - // for(unsigned int m = 0; run_search && m < modeldatabase->models.size(); m++){ - // printf("looking at: %i\n",int(modeldatabase->models[m]->last_changed)); - // reglib::Model * currentTest = modeldatabase->models[m]; - // if(currentTest->last_changed > current_model_update_before){ - // printf("changed: %i\n",int(m)); - - // double start = getTime(); - - // new_search_result = false; - // models_new_pub.publish(getModelMSG(currentTest)); - - // while(getTime()-start < search_timeout){ - // ros::spinOnce(); - // if(new_search_result){ - - // for(unsigned int i = 0; i < sresult.retrieved_images.size(); i++){ - - // int maxval = 0; - // int maxind = 0; - - // int dilation_size = 0; - // cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, - // cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), - // cv::Point( dilation_size, dilation_size ) ); - // for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ - // //framekeys - - // cv_bridge::CvImagePtr ret_image_ptr; - // try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv_bridge::CvImagePtr ret_mask_ptr; - // try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv_bridge::CvImagePtr ret_depth_ptr; - // try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv::Mat rgbimage = ret_image_ptr->image; - // cv::Mat maskimage = ret_mask_ptr->image; - // cv::Mat depthimage = ret_depth_ptr->image; - - // cv::Mat erosion_dst; - // cv::erode( maskimage, erosion_dst, element ); - - // unsigned short * depthdata = (unsigned short * )depthimage.data; - // unsigned char * rgbdata = (unsigned char * )rgbimage.data; - // unsigned char * maskdata = (unsigned char * )erosion_dst.data; - // int count = 0; - // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ - // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ - // count++; - // } - // } - // if(count > maxval){ - // maxval = count; - // maxind = j; - // } - // } - - // //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ - // if(maxval > 100){//Atleast some pixels has to be in the mask... - // std::string key = sresult.retrieved_image_paths[i].strings[maxind]; - // printf("searchresult key:%s\n",key.c_str()); - // if(framekeys.count(key) == 0){ - - // int j = maxind; - - // cv_bridge::CvImagePtr ret_image_ptr; - // try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv_bridge::CvImagePtr ret_mask_ptr; - // try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv_bridge::CvImagePtr ret_depth_ptr; - // try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv::Mat rgbimage = ret_image_ptr->image; - // cv::Mat maskimage = ret_mask_ptr->image; - // cv::Mat depthimage = ret_depth_ptr->image; - // for (int ii = 0; ii < depthimage.rows; ++ii) { - // for (int jj = 0; jj < depthimage.cols; ++jj) { - // depthimage.at(ii, jj) *= 5; - // } - // } - - - // cv::Mat erosion_dst; - // cv::erode( maskimage, erosion_dst, element ); - - // // cv::Mat overlap = rgbimage.clone(); - // // unsigned short * depthdata = (unsigned short * )depthimage.data; - // // unsigned char * rgbdata = (unsigned char * )rgbimage.data; - // // unsigned char * maskdata = (unsigned char * )maskimage.data; - // // unsigned char * overlapdata = (unsigned char * )overlap.data; - // // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ - // // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ - // // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; - // // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; - // // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; - // // }else{ - // // overlapdata[3*pixel+0] = 0; - // // overlapdata[3*pixel+1] = 0; - // // overlapdata[3*pixel+2] = 0; - // // } - // // } - // // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); - // // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); - // // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); - // // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); - - // Eigen::Affine3d epose = Eigen::Affine3d::Identity(); - // reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); - // frame->keyval = key; - - // // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); - // // frame->show(true); - // //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); - // reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); - // bool res = searchmodel->testFrame(0); - - // reglib::Model * searchmodelHolder = new reglib::Model(); - // searchmodelHolder->submodels.push_back(searchmodel); - // searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); - // searchmodelHolder->last_changed = ++current_model_update; - // searchmodelHolder->recomputeModelPoints(); - - // //searchmodelHolder->showHistory(viewer); - - - // // if(cv::waitKey(0) != 'n'){ - // //Todo:: check new model is not flat or L shape - - // printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); - // //addToDB(modeldatabase, searchmodel,false,true); - // addToDB(modeldatabase, searchmodelHolder,false,true); - // show_sorted(); - // } - // } - - - // /* - //// for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ - //// //framekeys - - //// cv_bridge::CvImagePtr ret_image_ptr; - //// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} - //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - //// cv_bridge::CvImagePtr ret_mask_ptr; - //// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} - //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - //// cv_bridge::CvImagePtr ret_depth_ptr; - //// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} - //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - //// cv::Mat rgbimage = ret_image_ptr->image; - //// cv::Mat maskimage = ret_mask_ptr->image; - //// cv::Mat depthimage = ret_depth_ptr->image; - - //// cv::Mat erosion_dst; - //// cv::erode( maskimage, erosion_dst, element ); - - //// unsigned short * depthdata = (unsigned short * )depthimage.data; - //// unsigned char * rgbdata = (unsigned char * )rgbimage.data; - //// unsigned char * maskdata = (unsigned char * )erosion_dst.data; - //// int count = 0; - //// for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ - //// if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ - //// count++; - //// } - //// } - //// if(count > maxval){ - //// maxval = count; - //// maxind = j; - //// } - //// } - - //// //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ - //// if(maxval > 100){//Atleast some pixels has to be in the mask... - //// std::string key = sresult.retrieved_image_paths[i].strings[maxind]; - //// printf("searchresult key:%s\n",key.c_str()); - //// if(framekeys.count(key) == 0){ - - //// int j = maxind; - - //// cv_bridge::CvImagePtr ret_image_ptr; - //// try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} - //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - //// cv_bridge::CvImagePtr ret_mask_ptr; - //// try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} - //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - //// cv_bridge::CvImagePtr ret_depth_ptr; - //// try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} - //// catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - //// cv::Mat rgbimage = ret_image_ptr->image; - //// cv::Mat maskimage = ret_mask_ptr->image; - //// cv::Mat depthimage = ret_depth_ptr->image; - //// for (int ii = 0; ii < depthimage.rows; ++ii) { - //// for (int jj = 0; jj < depthimage.cols; ++jj) { - //// depthimage.at(ii, jj) *= 5; - //// } - //// } - - - //// cv::Mat erosion_dst; - //// cv::erode( maskimage, erosion_dst, element ); - - //// // cv::Mat overlap = rgbimage.clone(); - //// // unsigned short * depthdata = (unsigned short * )depthimage.data; - //// // unsigned char * rgbdata = (unsigned char * )rgbimage.data; - //// // unsigned char * maskdata = (unsigned char * )maskimage.data; - //// // unsigned char * overlapdata = (unsigned char * )overlap.data; - //// // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ - //// // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ - //// // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; - //// // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; - //// // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; - //// // }else{ - //// // overlapdata[3*pixel+0] = 0; - //// // overlapdata[3*pixel+1] = 0; - //// // overlapdata[3*pixel+2] = 0; - //// // } - //// // } - //// // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); - //// // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); - //// // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); - //// // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); - - //// Eigen::Affine3d epose = Eigen::Affine3d::Identity(); - //// reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); - //// frame->keyval = key; - //// frame->show(true); - //// //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); - //// reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); - //// bool res = searchmodel->testFrame(0); - - //// reglib::Model * searchmodelHolder = new reglib::Model(); - //// searchmodelHolder->submodels.push_back(searchmodel); - //// searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); - //// searchmodelHolder->last_changed = ++current_model_update; - //// searchmodelHolder->recomputeModelPoints(); - - - //// // if(cv::waitKey(0) != 'n'){ - //// //Todo:: check new model is not flat or L shape - - //// printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); - //// //addToDB(modeldatabase, searchmodel,false,true); - //// addToDB(modeldatabase, searchmodelHolder,false,true); - //// show_sorted(); - //// // } - //// } - - //// } - // */ - // } - - // break; - // }else{ - // printf("searching... timeout in %3.3f\n", +start +search_timeout - getTime()); - // usleep(100000); - // } - // } - // } - // //exit(0); - // } - - // for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - // for(unsigned int j = 0; j < modeldatabase->models[i]->submodels.size(); j++){ - // if( modeldatabase->models[i]->submodels[j]->frames.front() == frontframe){ - // Eigen::Matrix4d pose = frontframe->pose * modeldatabase->models[i]->submodels_relativeposes[j].inverse(); - - // pcl::PointCloud::Ptr cloud = modeldatabase->models[i]->submodels[j]->getPCLcloud(1,true); - // pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); - // pcl::transformPointCloud (*cloud, *transformed_cloud, pose); - - - // sensor_msgs::PointCloud2 input; - // pcl::toROSMsg (*transformed_cloud,input);//, *transformed_cloud); - // input.header.frame_id = "/map"; - // model_last_pub.publish(input); - // } - // } - // } - - // newmodel = 0; - // sweepid_counter++; - +printf("%s::%i\n",__FILE__,__LINE__); } void modelCallback(const quasimodo_msgs::model & m){ + printf("%s::%i\n",__FILE__,__LINE__); quasimodo_msgs::model mod = m; - addNewModel(quasimodo_brain::getModelFromMSG(mod)); + addNewModel(quasimodo_brain::getModelFromMSG(mod,false)); } bool nextNew = true; @@ -1672,24 +1356,21 @@ int main(int argc, char **argv){ if(modeldatabase == 0){modeldatabase = new ModelDatabaseRetrieval(n);} - - if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/model/out/topic", 100, modelCallback));} + //if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/model/out/topic", 100, modelCallback));} + if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/quasimodo/segmentation/out/model", 100, modelCallback));} if(visualization){ - viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("viewer")); + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("Modelserver Viewer")); viewer->addCoordinateSystem(0.1); - viewer->setBackgroundColor(0.0,0.0,0.0); + viewer->setBackgroundColor(1.0,0.0,1.0); } ros::Duration(1.0).sleep(); chatter_pub = n.advertise("modelserver", 1000); -// sleep(1); std_msgs::String msg; msg.data = "starting"; chatter_pub.publish(msg); ros::spinOnce(); -// sleep(1); - ros::spin(); return 0; } diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index 750afa4b..fbe16377 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -54,7 +54,7 @@ namespace reglib std::vector< std::vector > intersections; RGBDFrame(); - RGBDFrame(Camera * camera_,cv::Mat rgb_, cv::Mat depth_, double capturetime_ = 0, Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity(), bool compute_normals = true, std::string savePath = ""); + RGBDFrame(Camera * camera_,cv::Mat rgb_, cv::Mat depth_, double capturetime_ = 0, Eigen::Matrix4d pose_ = Eigen::Matrix4d::Identity(), bool compute_normals = true, std::string savePath = "", bool compute_imgedges = true); ~RGBDFrame(); void show(bool stop = false); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 320d9819..b0c970e1 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -292,12 +292,44 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int return probs; } +//RGBDFrame * RGBDFrame::clone(){ +// return new RGBDFrame(camera->clone(), rgb.clone(),depth.clone(),capturetime, pose, true); +//} + RGBDFrame * RGBDFrame::clone(){ - return new RGBDFrame(camera->clone(), rgb.clone(),depth.clone(),capturetime, pose, true); +// printf("clone()\n"); +// exit(0); + RGBDFrame * frame = new RGBDFrame(); + frame->camera = camera->clone(); + frame->rgb = rgb.clone(); + frame->depth = depth.clone(); + frame->keyval = keyval; + frame->capturetime = capturetime; + frame->pose = pose; + frame->sweepid = sweepid; + frame->det_dilate = det_dilate.clone(); + frame->normals = normals.clone(); + frame->depthedges = depthedges.clone(); + frame->de = de.clone(); + frame->ce = ce.clone(); + frame->ce = ce.clone(); + frame->ce = ce.clone(); + + unsigned int width = camera->width; + unsigned int height = camera->height; + unsigned int nr_pixels = width*height; + + frame->connections = connections; + frame->intersections = intersections; + frame->nr_labels = nr_labels; + frame->labels = new int[nr_pixels]; + for(int i = 0; i < nr_pixels; i++){frame->labels[i] = labels[i];} + + return frame;//new RGBDFrame(camera->clone(), rgb.clone(),depth.clone(),capturetime, pose, true); } -RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals, std::string savePath){ - printf("savepath: %s\n",savePath.c_str()); +RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals, std::string savePath, bool compute_imgedges){ + printf("savepath: %s\n",savePath.c_str()); bool verbose = false; if(verbose) printf("------------------------------\n"); @@ -321,7 +353,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt if(savePath.size() != 0){ cv::imwrite( std::string(savestr)+"rgb.png", rgb ); cv::imwrite( std::string(savestr)+"depth.png", depth ); - } + } IplImage iplimg = rgb_; IplImage* img = &iplimg; @@ -358,6 +390,14 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float * dedata = (float*)de.data; for(int i = 0; i < 3*nr_pixels; i++){dedata[i] = 0;} + ce.create(height,width,CV_32FC3); + float * cedata = (float*)ce.data; + for(int i = 0; i < 3*nr_pixels; i++){cedata[i] = 0;} + + depthedges.create(height,width,CV_8UC1); + unsigned char * depthedgesdata = (unsigned char *)depthedges.data; + for(int i = 0; i < nr_pixels; i++){depthedgesdata[i] = 0;} +if(compute_imgedges){ std::vector Xvec; int step = 1; for(unsigned int w = step; w < width-step;w++){ @@ -484,103 +524,10 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt float * bedata = (float*)be.data; for(int i = 0; i < 3*nr_pixels; i++){bedata[i] = 0;} - ce.create(height,width,CV_32FC3); - float * cedata = (float*)ce.data; - for(int i = 0; i < 3*nr_pixels; i++){cedata[i] = 0;} - std::vector XvecR; std::vector XvecG; std::vector XvecB; -// for(unsigned int w = step; w < width-step;w++){ -// for(unsigned int h = step; h < height-step;h++){ -// int ind = h*width+w; -// float b = float(blurdata[3*ind+0]); -// float g = float(blurdata[3*ind+1]); -// float r = float(blurdata[3*ind+2]); - -// if(w > 2 && w < width-1){ -// float b0 = float(blurdata[3*(ind-2)+0]); -// float b1 = float(blurdata[3*(ind-1)+0]); -// float b2 = b; -// float b3 = float(blurdata[3*(ind+1)+0]); - - -// float bpred1 = b1 + (b1-b0); -// float bpred2 = b2 - (b3-b2); - -// //printf("%3.3f %3.3f %3.3f %3.3f -> %3.3f %3.3f -> %3.3f %3.3f -> %3.3f\n",b0,b1,b2,b3,bpred1,bpred2,fabs(b2-bpred1),fabs(b1-bpred2),fabs(b2-bpred1) + fabs(b1-bpred2)); - -// bedata[3*ind+1] = 0.5*(fabs(b2-bpred1) + fabs(b1-bpred2)); -// XvecB.push_back(bedata[3*ind+1]); - - -// float g0 = float(blurdata[3*(ind-2)+1]); -// float g1 = float(blurdata[3*(ind-1)+1]); -// float g2 = g; -// float g3 = float(blurdata[3*(ind+1)+1]); - -// float gpred1 = g1 + (g1-g0); -// float gpred2 = g2 - (g3-g2); - -// gedata[3*ind+1] = 0.5*(fabs(g2-gpred1) + fabs(g1-gpred2)); -// XvecG.push_back(gedata[3*ind+1]); - - -// float r0 = float(blurdata[3*(ind-2)+2]); -// float r1 = float(blurdata[3*(ind-1)+2]); -// float r2 = r; -// float r3 = float(blurdata[3*(ind+1)+2]); - -// float rpred1 = r1 + (r1-r0); -// float rpred2 = r2 - (r3-r2); - -// redata[3*ind+1] = 0.5*(fabs(r2-rpred1) + fabs(r1-rpred2)); -// XvecR.push_back(redata[3*ind+1]); - - -// } - -// if(h > 2 && h < height-1){ -// float b0 = float(blurdata[3*(ind-2*width)+0]); -// float b1 = float(blurdata[3*(ind-1*width)+0]); -// float b2 = b; -// float b3 = float(blurdata[3*(ind+1*width)+0]); - -// float bpred1 = b1 + (b1-b0); -// float bpred2 = b2 - (b3-b2); - -// bedata[3*ind+2] = 0.5*(fabs(b2-bpred1) + fabs(b1-bpred2)); -// XvecB.push_back(bedata[3*ind+2]); - - -// float g0 = float(blurdata[3*(ind-2*width)+1]); -// float g1 = float(blurdata[3*(ind-1*width)+1]); -// float g2 = g; -// float g3 = float(blurdata[3*(ind+1*width)+1]); - -// float gpred1 = g1 + (g1-g0); -// float gpred2 = g2 - (g3-g2); - -// gedata[3*ind+2] = 0.5*(fabs(g2-gpred1) + fabs(g1-gpred2)); -// XvecG.push_back(gedata[3*ind+2]); - - -// float r0 = float(blurdata[3*(ind-2*width)+2]); -// float r1 = float(blurdata[3*(ind-1*width)+2]); -// float r2 = r; -// float r3 = float(blurdata[3*(ind+1*width)+2]); - -// float rpred1 = r1 + (r1-r0); -// float rpred2 = r2 - (r3-r2); - -// redata[3*ind+2] = 0.5*(fabs(r2-rpred1) + fabs(r1-rpred2)); -// XvecR.push_back(redata[3*ind+2]); - -// } -// } -// } - for(unsigned int w = 1; w < width-1;w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; @@ -608,33 +555,6 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt } } -// for(unsigned int w = 1; w < width-1;w++){ -// for(unsigned int h = 0; h < height;h++){ -// int ind = h*width+w; -// bedata[3*ind+1] = fabs(blurdata[3*(ind+0)+0]-0.5*(blurdata[3*(ind+1)+0] + blurdata[3*(ind-1)+0])); -// XvecB.push_back(bedata[3*ind+1]); - -// gedata[3*ind+1] = fabs(blurdata[3*(ind+0)+1]-0.5*(blurdata[3*(ind+1)+1] + blurdata[3*(ind-1)+1])); -// XvecG.push_back(gedata[3*ind+1]); - -// redata[3*ind+1] = fabs(blurdata[3*(ind+0)+2]-0.5*(blurdata[3*(ind+1)+2] + blurdata[3*(ind-1)+2])); -// XvecR.push_back(redata[3*ind+1]); -// } -// } -// for(unsigned int w = 0; w < width;w++){ -// for(unsigned int h = 1; h < height-1;h++){ -// int ind = h*width+w; -// bedata[3*ind+2] = fabs(blurdata[3*(ind+0)+0]-0.5*(blurdata[3*(ind+width)+0] + blurdata[3*(ind-width)+0])); -// XvecB.push_back(bedata[3*ind+1]); - -// gedata[3*ind+2] = fabs(blurdata[3*(ind+0)+1]-0.5*(blurdata[3*(ind+width)+1] + blurdata[3*(ind-width)+1])); -// XvecG.push_back(gedata[3*ind+1]); - -// redata[3*ind+2] = fabs(blurdata[3*(ind+0)+2]-0.5*(blurdata[3*(ind+width)+2] + blurdata[3*(ind-width)+2])); -// XvecR.push_back(redata[3*ind+1]); -// } -// } - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_diff.png", 3.0*re );} if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_diff.png", 3.0*ge );} @@ -774,10 +694,6 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt delete funcR; delete funcG; delete funcB; -// delete ggdfuncR; -// delete ggdfuncG; -// delete ggdfuncB; -//exit(0); if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_prob.png", 127.0*re );} if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_prob.png", 127.0*ge );} @@ -827,12 +743,13 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) ); unsigned char * det_dilatedata = (unsigned char*)det_dilate.data; - depthedges.create(height,width,CV_8UC1); - unsigned char * depthedgesdata = (unsigned char *)depthedges.data; +// depthedges.create(height,width,CV_8UC1); +// unsigned char * depthedgesdata = (unsigned char *)depthedges.data; for(int i = 0; i < nr_pixels; i++){ //depthedgesdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5) || (cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5)); depthedgesdata[i] = 255*(((cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5)) && (det_dilatedata[i] == 0)); } +} // cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges); // cv::waitKey(0); @@ -924,7 +841,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); printf("complete time to create RGBD image: %5.5fs\n",getTime()-completeStartTime); - //getImageProbs(); + //getImageProbs(); if(savePath.size() != 0){ saveCombinedImages(std::string(savestr)+"Combined.png"); saveCombinedProcessedImages(std::string(savestr)+"CombinedProcessed.png"); @@ -1134,7 +1051,7 @@ RGBDFrame * RGBDFrame::load(Camera * cam, std::string path){ size = file.tellg(); buffer = new char [size]; file.seekg (0, std::ios::beg); - file.read (buffer, size); + file.read (buffer, size); file.close(); double * buffer_double = (double *)buffer; @@ -1317,11 +1234,11 @@ std::vector< std::vector > RGBDFrame::getImageProbs(bool depthonly){ if(!depthonly){ if(!det_dilatedata[i]){ - dxc[i] = 1.0-std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); - dyc[i] = 1.0-std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); + dxc[i] = 1.0-std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); + dyc[i] = 1.0-std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); }else{ - dxc[i] = 1.0-std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); - dyc[i] = 1.0-std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); + dxc[i] = 1.0-std::max(dedata[3*i+1],0.8f*cedata[3*i+1]); + dyc[i] = 1.0-std::max(dedata[3*i+2],0.8f*cedata[3*i+2]); } }else{ dxc[i] = 1.0-dedata[3*i+1]; @@ -1343,56 +1260,56 @@ std::vector< std::vector > RGBDFrame::getImageProbs(bool depthonly){ } void RGBDFrame::saveCombinedImages(std::string path){ - unsigned int width = camera->width; - unsigned int height = camera->height; - cv::Mat combined; - combined.create(camera->height,2*camera->width,CV_8UC3); - unsigned char * combineddata = combined.data; - unsigned char * rgbdata = rgb.data; - unsigned short * depthdata = (unsigned short *)(depth.data); - - for(unsigned long h = 0; h < height;h++){ - for(unsigned long w = 0; w < width;w++){ - combineddata[3*(h*(2*width)+w)+0] = rgbdata[3*(h*width+w)+0]; - combineddata[3*(h*(2*width)+w)+1] = rgbdata[3*(h*width+w)+1]; - combineddata[3*(h*(2*width)+w)+2] = rgbdata[3*(h*width+w)+2]; - combineddata[3*(h*(2*width)+w+width)+0] = depthdata[h*width+w]/256; - combineddata[3*(h*(2*width)+w+width)+1] = depthdata[h*width+w]/256; - combineddata[3*(h*(2*width)+w+width)+2] = depthdata[h*width+w]/256; - } - } + unsigned int width = camera->width; + unsigned int height = camera->height; + cv::Mat combined; + combined.create(camera->height,2*camera->width,CV_8UC3); + unsigned char * combineddata = combined.data; + unsigned char * rgbdata = rgb.data; + unsigned short * depthdata = (unsigned short *)(depth.data); + + for(unsigned long h = 0; h < height;h++){ + for(unsigned long w = 0; w < width;w++){ + combineddata[3*(h*(2*width)+w)+0] = rgbdata[3*(h*width+w)+0]; + combineddata[3*(h*(2*width)+w)+1] = rgbdata[3*(h*width+w)+1]; + combineddata[3*(h*(2*width)+w)+2] = rgbdata[3*(h*width+w)+2]; + combineddata[3*(h*(2*width)+w+width)+0] = depthdata[h*width+w]/256; + combineddata[3*(h*(2*width)+w+width)+1] = depthdata[h*width+w]/256; + combineddata[3*(h*(2*width)+w+width)+2] = depthdata[h*width+w]/256; + } + } cv::imwrite( path, combined ); } void RGBDFrame::saveCombinedProcessedImages(std::string path){ - unsigned int width = camera->width; - unsigned int height = camera->height; - cv::Mat combined; - combined.create(camera->height,3*camera->width,CV_8UC3); - unsigned char * combineddata = combined.data; - float * normalsdata = (float *)(normals.data); - float * cedata = (float *)(ce.data); - float * dedata = (float *)(de.data); - - for(unsigned long h = 0; h < height;h++){ - for(unsigned long w = 0; w < width;w++){ + unsigned int width = camera->width; + unsigned int height = camera->height; + cv::Mat combined; + combined.create(camera->height,3*camera->width,CV_8UC3); + unsigned char * combineddata = combined.data; + float * normalsdata = (float *)(normals.data); + float * cedata = (float *)(ce.data); + float * dedata = (float *)(de.data); + + for(unsigned long h = 0; h < height;h++){ + for(unsigned long w = 0; w < width;w++){ if(normalsdata[3*(h*width+w)+0] <= 1){ combineddata[3*(h*(3*width)+w+0*width)+0] = 255.0*fabs(normalsdata[3*(h*width+w)+0]); combineddata[3*(h*(3*width)+w+0*width)+1] = 255.0*fabs(normalsdata[3*(h*width+w)+1]); combineddata[3*(h*(3*width)+w+0*width)+2] = 255.0*fabs(normalsdata[3*(h*width+w)+2]); - }else{ - combineddata[3*(h*(3*width)+w+0*width)+0] = 0; - combineddata[3*(h*(3*width)+w+0*width)+1] = 0; - combineddata[3*(h*(3*width)+w+0*width)+2] = 0; - } - combineddata[3*(h*(3*width)+w+1*width)+0] = 255.0*cedata[3*(h*width+w)+0]; - combineddata[3*(h*(3*width)+w+1*width)+1] = 255.0*cedata[3*(h*width+w)+1]; - combineddata[3*(h*(3*width)+w+1*width)+2] = 255.0*cedata[3*(h*width+w)+2]; - combineddata[3*(h*(3*width)+w+2*width)+0] = 255.0*dedata[3*(h*width+w)+0]; - combineddata[3*(h*(3*width)+w+2*width)+1] = 255.0*dedata[3*(h*width+w)+1]; - combineddata[3*(h*(3*width)+w+2*width)+2] = 255.0*dedata[3*(h*width+w)+2]; - } - } + }else{ + combineddata[3*(h*(3*width)+w+0*width)+0] = 0; + combineddata[3*(h*(3*width)+w+0*width)+1] = 0; + combineddata[3*(h*(3*width)+w+0*width)+2] = 0; + } + combineddata[3*(h*(3*width)+w+1*width)+0] = 255.0*cedata[3*(h*width+w)+0]; + combineddata[3*(h*(3*width)+w+1*width)+1] = 255.0*cedata[3*(h*width+w)+1]; + combineddata[3*(h*(3*width)+w+1*width)+2] = 255.0*cedata[3*(h*width+w)+2]; + combineddata[3*(h*(3*width)+w+2*width)+0] = 255.0*dedata[3*(h*width+w)+0]; + combineddata[3*(h*(3*width)+w+2*width)+1] = 255.0*dedata[3*(h*width+w)+1]; + combineddata[3*(h*(3*width)+w+2*width)+2] = 255.0*dedata[3*(h*width+w)+2]; + } + } cv::imwrite( path, combined ); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index 5329732a..cb180bd6 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -452,7 +452,7 @@ void ModelUpdater::makeInitialSetup(){ return ; } - MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.05); + MassRegistrationPPR2 * massreg = new MassRegistrationPPR2(0.0); massreg->timeout = 4*massreg_timeout; massreg->viewer = viewer; massreg->visualizationLvl = show_init_lvl; From 15f5a822fda0c3e521fbfe04a9fb9ccd61b5e10f Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 7 Nov 2016 15:51:36 +0100 Subject: [PATCH 203/255] fix to compilation error --- .../src/ModelDatabase/ModelDatabaseRetrieval.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp index 5c2a3b59..a3020832 100644 --- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp +++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp @@ -85,13 +85,13 @@ std::vector ModelDatabaseRetrieval::search(reglib::Model * mode printf("---------------ids in searchresult: %i----------------\n",result.vocabulary_ids.size()); for(unsigned int i = 0; i < result.vocabulary_ids.size(); i++){ - int ind = result.vocabulary_ids[i]; + int ind = std::stoi(result.vocabulary_ids[i]); printf("found object with ind: %i\n",ind); } printf("----------------------------------------------\n"); for(unsigned int i = 0; i < result.vocabulary_ids.size(); i++){ - int ind = result.vocabulary_ids[i]; + int ind = std::stoi(result.vocabulary_ids[i]); printf("found object with ind: %i\n",ind); if(vocabulary_ids.count(ind) == 0){ printf("does not exist in db, continue\n"); continue; } reglib::Model * search_model = vocabulary_ids[ind]; From 2ea84bcb6cda59afcc88f0771f3c8a691753b78a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 7 Nov 2016 16:11:30 +0100 Subject: [PATCH 204/255] added reaction to additional view output --- .../src/metaroom_additional_view_processing.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 5118d55a..72220d47 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1519,9 +1519,13 @@ void sendMetaroomToServer(std::string path){ } } - void sendCallback(const std_msgs::String::ConstPtr& msg){sendMetaroomToServer(msg->data);} +void processAndSendCallback(const std_msgs::String::ConstPtr& msg){ + processMetaroom(msg->data); + sendCallback(msg); +} + bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res){ printf("bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res)\n"); std::string current_waypointid = req.waypoint_id; @@ -1728,6 +1732,8 @@ int main(int argc, char** argv){ std::vector segsubs; std::vector sendsubs; std::vector roomObservationSubs; + std::vector processAndSendsubs; + std::vector trainMetarooms; std::vector sendMetaroomToServers; @@ -1840,6 +1846,10 @@ int main(int argc, char** argv){ if(roomObservationSubs.size() == 0){ roomObservationSubs.push_back(n.subscribe("/local_metric_map/room_observations", 1000, roomObservationCallback)); } + + if(processAndSendsubs.size() == 0){ + processAndSendsubs.push_back(n.subscribe("/object_learning/learned_object_xml", 1000, processAndSendCallback)); + } } printf("overall_folder: %s\n",overall_folder.c_str()); From 9c9b997a74211482c60a8b68723d2f9411ea890c Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 8 Nov 2016 13:20:23 +0100 Subject: [PATCH 205/255] ... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 81 +++++++++++++++++++ quasimodo/quasimodo_brain/src/Util/Util.h | 1 + .../metaroom_additional_view_processing.cpp | 14 ++-- quasimodo/quasimodo_brain/src/modelserver.cpp | 12 ++- 4 files changed, 101 insertions(+), 7 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 2f29661d..774a9190 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -540,4 +540,85 @@ void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > } */ +std::vector getFileList(std::string path){ + std::vector files; + + DIR *dp; + struct dirent *ep; + dp = opendir (path.c_str()); + if (dp != NULL) { + while (ep = readdir (dp)){ + if (ep->d_type != DT_DIR) { + files.push_back(std::string(ep->d_name)); + } + } + closedir (dp); + }else{ + printf("Couldn't open %s\n",path.c_str()); + } + + return files; +} + +std::vector getFolderList(std::string path){ + std::vector files; + + DIR *dp; + struct dirent *ep; + dp = opendir (path.c_str()); + if (dp != NULL) { + while (ep = readdir (dp)){ + std::string data = std::string(ep->d_name); + if (data.compare(".") != 0 && data.compare("..") != 0 && ep->d_type == DT_DIR) { + files.push_back(data); + } + } + closedir (dp); + }else{ + printf("Couldn't open %s\n",path.c_str()); + } + return files; +} + +void recursiveGetFolderList(std::vector & folders, std::string path){ + std::vector list = getFolderList(path); + for(unsigned int i = 0; i < list.size(); i++){ + folders.push_back(path+"/"+list[i]); + recursiveGetFolderList(folders, path+"/"+list[i]); + } +} + +std::vector recursiveGetFolderList(std::string path){ + std::vector folders; + recursiveGetFolderList(folders, path); + return folders; +} + +std::vector loadModelsPCDs(std::string path){ + std::vector models; + + std::vector folders = getFolderList(path);//recursiveGetFolderList(path);//getFolderList(path);// + for(unsigned int i = 0; i < folders.size(); i++){ + printf("folders: %s\n",folders[i].c_str()); + std::string views = path+folders[i]+"/views"; + std::vector files = getFileList(views); + + std::vector cloudspaths; + std::vector indexpaths; + std::vector posepaths; + + for(unsigned int j = 0; j < files.size(); j++){ + std::string file = views+"/"+files[j]; + printf("Files: %s\n",file.c_str()); + if ((files[j].find("cloud") != std::string::npos) && (files[j].find(".pcd") !=std::string::npos)){cloudspaths.push_back(file);} + if (files[j].find("indices") != std::string::npos){indexpaths.push_back(file);} + if (files[j].find("pose") != std::string::npos){posepaths.push_back(file);} + } + + printf("%i %i %i\n",cloudspaths.size(),indexpaths.size(),posepaths.size()); + } + + return models; +} + } diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index cef25163..0ee2fe11 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -57,6 +57,7 @@ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > models, std::vector< std::vector< cv::Mat > > & internal, std::vector< std::vector< cv::Mat > > & external, std::vector< std::vector< cv::Mat > > & dynamic, int debugg = 0, std::string savePath = ""); std::vector loadModelsXML(std::string path); +std::vector loadModelsPCDs(std::string path); } diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 72220d47..5b1808fa 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1522,6 +1522,9 @@ void sendMetaroomToServer(std::string path){ void sendCallback(const std_msgs::String::ConstPtr& msg){sendMetaroomToServer(msg->data);} void processAndSendCallback(const std_msgs::String::ConstPtr& msg){ + printf("================================================================================\n"); + printf("============================processAndSendCallback==============================\n"); + printf("================================================================================\n"); processMetaroom(msg->data); sendCallback(msg); } @@ -1769,8 +1772,8 @@ int main(int argc, char** argv){ else if(std::string(argv[i]).compare("-nobase") == 0){ baseSetting = false;} else if(std::string(argv[i]).compare("-recomputeRelativePoses") == 0){ recomputeRelativePoses = true;} else if(std::string(argv[i]).compare("-v_init") == 0){ visualization_lvl_regini = 1;inputstate = 17;} - else if(std::string(argv[i]).compare("-v_reg") == 0){ visualization_lvl_regref = 1;inputstate = 18;} - else if(std::string(argv[i]).compare("-saveVisuals") == 0){inputstate = 19;} + else if(std::string(argv[i]).compare("-v_reg") == 0){ visualization_lvl_regref = 1;inputstate = 18;} + else if(std::string(argv[i]).compare("-saveVisuals") == 0){ inputstate = 19;} else if(inputstate == 0){ segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); }else if(inputstate == 1){ @@ -1810,11 +1813,12 @@ int main(int argc, char** argv){ visualization_lvl_regini = atoi(argv[i]); }else if(inputstate == 18){ visualization_lvl_regref = atoi(argv[i]); - }else if(inputstate == 19){ - saveVisuals = std::string(argv[i]); - } + }else if(inputstate == 19){ + saveVisuals = std::string(argv[i]); + } } + if(visualization_lvl != 0 || visualization_lvl_regref != 0 || visualization_lvl_regini != 0){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); viewer->setBackgroundColor (0.5, 0, 0.5); diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index bca99ac4..a3870085 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -1278,6 +1278,7 @@ int main(int argc, char **argv){ std::vector input_model_subs; + std::vector modelpcds; int inputstate = -1; for(int i = 1; i < argc;i++){ @@ -1302,9 +1303,10 @@ int main(int argc, char **argv){ else if(std::string(argv[i]).compare("-v_scoring") == 0 || std::string(argv[i]).compare("-v_score") == 0 || std::string(argv[i]).compare("-v_sco") == 0){ printf("visualization scoring turned on\n"); visualization = true; show_scoring = true;} else if(std::string(argv[i]).compare("-v_db") == 0){ printf("visualization db turned on\n"); visualization = true; show_db = true;} else if(std::string(argv[i]).compare("-intopic") == 0){ printf("intopic input state\n"); inputstate = 11;} - else if(std::string(argv[i]).compare("-mdb") == 0){ printf("intopic input state\n"); inputstate = 12;} - else if(std::string(argv[i]).compare("-show_search") == 0){ printf("show_search\n"); show_search = true;} + else if(std::string(argv[i]).compare("-mdb") == 0){ printf("intopic input state\n"); inputstate = 12;} + else if(std::string(argv[i]).compare("-show_search") == 0){ printf("show_search\n"); show_search = true;} else if(std::string(argv[i]).compare("-show_modelbuild") == 0){ printf("show_modelbuild\n"); visualization = true; show_modelbuild = true;} + else if(std::string(argv[i]).compare("-loadModelsPCDs") == 0){ inputstate = 13;} else if(inputstate == 1){ reglib::Camera * cam = reglib::Camera::load(std::string(argv[i])); delete cameras[0]; @@ -1351,11 +1353,17 @@ int main(int argc, char **argv){ if(modeldatabase != 0){delete modeldatabase;} modeldatabase = new ModelDatabaseRetrieval(n); } + }else if(inputstate == 13){ + modelpcds.push_back( std::string(argv[i]) ); } } if(modeldatabase == 0){modeldatabase = new ModelDatabaseRetrieval(n);} + for(unsigned int i = 0; i < modelpcds.size(); i++){ + std::vector mods = quasimodo_brain::loadModelsPCDs(modelpcds[i]); + } + //if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/model/out/topic", 100, modelCallback));} if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/quasimodo/segmentation/out/model", 100, modelCallback));} From 92748bc0d2fb04ea9be23abdf97903266b139c43 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 8 Nov 2016 14:32:25 +0100 Subject: [PATCH 206/255] pre loading of pcd objects added --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 164 +++++++++++++++++- quasimodo/quasimodo_brain/src/modelserver.cpp | 4 + 2 files changed, 161 insertions(+), 7 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 774a9190..132ffc5b 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -76,11 +76,11 @@ reglib::Model * loadFromRaresFormat(std::string path){ } -// if(model->relativeposes.size() == 0){ -// model->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); -// }else{ -// model->submodels_relativeposes.push_back(model->relativeposes.front().inverse() * sweep->frames.front()->pose); -// } + // if(model->relativeposes.size() == 0){ + // model->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + // }else{ + // model->submodels_relativeposes.push_back(model->relativeposes.front().inverse() * sweep->frames.front()->pose); + // } return model; @@ -263,8 +263,9 @@ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath) dummy.push_back("RoomIntermediateCloud"); dummy.push_back("IntermediatePosition"); SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml",dummy); -printf("loaded room XML\n"); + printf("loaded room XML: %s\n",(sweep_folder+"/room.xml").c_str()); reglib::Model * sweepmodel = 0; + printf("%i\n",roomData.vIntermediateRoomCloudTransforms.size()); Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); std::vector current_room_frames; @@ -292,7 +293,7 @@ printf("loaded room XML\n"); Eigen::Matrix4d m = m2*getMat(roomData.vIntermediateRoomCloudTransformsRegistered[i]); //Eigen::Matrix4d m = getMat(roomData.vIntermediateRoomCloudTransforms[i]); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, m,true,savePath); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, m,true,savePath); current_room_frames.push_back(frame); if(sweepmodel == 0){ @@ -594,6 +595,51 @@ std::vector recursiveGetFolderList(std::string path){ return folders; } +Eigen::Matrix4f getRegisteredViewPoses(const std::string& poses_file){ + Eigen::Matrix4f transform = Eigen::Matrix4f::Identity(); + std::ifstream in(poses_file); + if (!in.is_open()){ + cout<<"ERROR: cannot find poses file "<> temp; + transform(j,k) = temp; + } + } + in.close(); + return transform; +} + +std::vector getIndsFromFile(const std::string& poses_file){ + std::vector indexes; + std::ifstream in(poses_file); + if (!in.is_open()){ + cout<<"ERROR: cannot find idnex file "< loadModelsPCDs(std::string path){ std::vector models; @@ -616,6 +662,110 @@ std::vector loadModelsPCDs(std::string path){ } printf("%i %i %i\n",cloudspaths.size(),indexpaths.size(),posepaths.size()); + if((cloudspaths.size() == indexpaths.size()) && (cloudspaths.size() == posepaths.size())){ + std::sort (cloudspaths.begin(), cloudspaths.end()); + std::sort (indexpaths.begin(), indexpaths.end()); + std::sort (posepaths.begin(), posepaths.end()); + + + reglib::Model * model = new reglib::Model(); + +// for(unsigned int i = 0; i < msg.local_poses.size(); i++){ +// sensor_msgs::CameraInfo camera = msg.frames[i].camera; +// ros::Time capture_time = msg.frames[i].capture_time; +// geometry_msgs::Pose pose = msg.frames[i].pose; + +// cv_bridge::CvImagePtr rgb_ptr; +// try{ rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);} +// catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} +// cv::Mat rgb = rgb_ptr->image; + +// cv_bridge::CvImagePtr depth_ptr; +// try{ depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);} +// catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} +// cv::Mat depth = depth_ptr->image; + +// Eigen::Affine3d epose; +// tf::poseMsgToEigen(pose, epose); + +// reglib::Camera * cam = new reglib::Camera(); +// if(camera.K[0] > 0){ +// cam->fx = camera.K[0]; +// cam->fy = camera.K[4]; +// cam->cx = camera.K[2]; +// cam->cy = camera.K[5]; +// } + +// } +// + + for(unsigned int j = 0; j < cloudspaths.size(); j++){ + std::string cloudpath = cloudspaths[j]; + std::string indexpath = indexpaths[j]; + std::string posepath = posepaths[j]; + printf("loading: %s %s %s\n", cloudpath.c_str(),indexpath.c_str(),posepath.c_str()); + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + if (pcl::io::loadPCDFile (cloudpath, *cloud) != -1){ + Eigen::Matrix4f pose = getRegisteredViewPoses(posepath); + std::vector indexes = getIndsFromFile(indexpath); + std::cout << pose << std::endl << std::endl; + std::cout << indexes.size() << std::endl; + + unsigned int width = cloud->width; + unsigned int height = cloud->height; + cv::Mat rgb; + rgb.create(height,width,CV_8UC3); + unsigned char * rgbdata = rgb.data; + + cv::Mat depth; + depth.create(height,width,CV_16UC1); + unsigned short * depthdata = (unsigned short *)(depth.data); + + reglib::Camera * cam = new reglib::Camera();//figure out cam params + + //TODO figure out optical centre and focal lengths from cloud... +// for(unsigned int w = 0; w < width; w++){ +// for(unsigned int w = 0; w < width; w++){ +// pcl::PointXYZRGB p = cloud->points[k]; +// float x = p.x; +// float y = p.y; +// float z = p.z; +// } +// } + + for(unsigned int k = 0; k < width*height; k++){ + pcl::PointXYZRGB p = cloud->points[k]; + rgbdata[3*k+0] = p.b; + rgbdata[3*k+1] = p.g; + rgbdata[3*k+2] = p.r; + if(!std::isnan(p.z)){ + depthdata[k] = 5000*p.z; + }else{ + depthdata[k] = 0; + } + } + + + cv::Mat mask; + mask.create(height,width,CV_8UC1); + unsigned char * maskdata = mask.data; + for(unsigned int k = 0; k < indexes.size(); k++){maskdata[indexes[k]] = 255;} + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, 0 , pose.cast() , true,"",false); + model->frames.push_back(frame); + model->relativeposes.push_back(model->frames.front()->pose.inverse() * pose.cast()); + model->modelmasks.push_back(new reglib::ModelMask(mask)); + }else{ + printf ("Couldn't read pcd file\n"); + } + } + + model->recomputeModelPoints(); + models.push_back(model); + }else{ + printf("number of clouds, indices and poses dont match... ignoring\n"); + } } return models; diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index a3870085..32ec18b3 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -1362,8 +1362,12 @@ int main(int argc, char **argv){ for(unsigned int i = 0; i < modelpcds.size(); i++){ std::vector mods = quasimodo_brain::loadModelsPCDs(modelpcds[i]); + for(unsigned int j = 0; j < mods.size(); j++){ + modeldatabase->add(mods[j]); + } } + //if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/model/out/topic", 100, modelCallback));} if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/quasimodo/segmentation/out/model", 100, modelCallback));} From 200c4966d99f76117b887b973ebb2658f25f2e31 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 8 Nov 2016 16:18:49 +0100 Subject: [PATCH 207/255] ... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 10 +++++++++ .../metaroom_additional_view_processing.cpp | 22 ++++++++++++++++++- .../include/model/ModelMask.h | 3 ++- .../quasimodo_models/src/model/Model.cpp | 4 ++-- .../quasimodo_models/src/model/ModelMask.cpp | 3 ++- 5 files changed, 37 insertions(+), 5 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 132ffc5b..c9c1d915 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -21,6 +21,10 @@ void sortXMLs(std::vector & sweeps){ reglib::Model * loadFromRaresFormat(std::string path){ reglib::Model * model = new reglib::Model(); reglib::Model * sweep = load_metaroom_model(path); + if(sweep->frames.size() == 0){ + printf("no frames in sweep, returning\n"); + return sweep; + } model->submodels.push_back(sweep); model->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); @@ -264,6 +268,12 @@ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath) dummy.push_back("IntermediatePosition"); SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml",dummy); printf("loaded room XML: %s\n",(sweep_folder+"/room.xml").c_str()); + + if(roomData.vIntermediateRoomClouds.size() == 0 ){ + printf("WARNING:: no clouds in room\n"); + return new reglib::Model(); + } + reglib::Model * sweepmodel = 0; printf("%i\n",roomData.vIntermediateRoomCloudTransforms.size()); Eigen::Matrix4d m2 = getMat(roomData.vIntermediateRoomCloudTransforms[0]); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 5b1808fa..b9341313 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -648,6 +648,11 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri // } reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); + if(sweep->frames.size() == 0){ + printf("no frames in sweep, returning\n"); + return sweep; + } + for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; sweep->relativeposes[i] = sweepPoses[i]; @@ -960,6 +965,8 @@ int processMetaroom(std::string path, bool store_old_xml = true){ printf("fullmodel->frames.size() = %i\n",fullmodel->frames.size()); + + // fullmodel->fullDelete(); // delete fullmodel; // return 0; @@ -975,6 +982,13 @@ int processMetaroom(std::string path, bool store_old_xml = true){ std::vector fr; std::vector mm; fullmodel->getData(po, fr, mm); + + if(fr.size() == 0){ + printf("no frames in model, returning\n"); + fullmodel->fullDelete(); + delete fullmodel; + return 0; + } // fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); // fullmodel->recomputeModelPoints(); // SemanticRoom observation = SemanticRoomXMLParser::loadRoomFromXML(path,false); @@ -1029,7 +1043,13 @@ int processMetaroom(std::string path, bool store_old_xml = true){ std::string prev = sweep_xmls[prevind]; printf("prev: %s\n",prev.c_str()); reglib::Model * bg = getAVMetaroom(prev); - bgs.push_back(bg); + if(bg->frames.size() == 0){ + printf("no frames in bg\n"); + bg->fullDelete(); + delete bg; + }else{ + bgs.push_back(bg); + } }else{ printf("no previous...\n"); } diff --git a/quasimodo/quasimodo_models/include/model/ModelMask.h b/quasimodo/quasimodo_models/include/model/ModelMask.h index a75ecf88..cb5cb4fb 100644 --- a/quasimodo/quasimodo_models/include/model/ModelMask.h +++ b/quasimodo/quasimodo_models/include/model/ModelMask.h @@ -19,6 +19,7 @@ namespace reglib class ModelMask{ public: int id; + std::string label; cv::Mat mask; int width; int height; @@ -27,7 +28,7 @@ namespace reglib std::vector testh; int sweepid; - ModelMask(cv::Mat mask_); + ModelMask(cv::Mat mask_, std::string label_ = ""); ~ModelMask(); cv::Mat getMask(); }; diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 57fde321..8354aff0 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -29,7 +29,7 @@ Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){ relativeposes.push_back(pose); frames.push_back(frame); - modelmasks.push_back(new ModelMask(mask)); + modelmasks.push_back(new ModelMask(mask)); savePath = ""; //recomputeModelPoints(); @@ -589,7 +589,7 @@ pcl::PointCloud::Ptr Model::getPCLcloud(int step, bool color){ }else{ p.b = sp.feature(0); p.g = sp.feature(1); - p.r = sp.feature(2); + p.b = sp.feature(2); } cloud_ptr->points.push_back(p); } diff --git a/quasimodo/quasimodo_models/src/model/ModelMask.cpp b/quasimodo/quasimodo_models/src/model/ModelMask.cpp index 1c79a0d1..d1da1c71 100644 --- a/quasimodo/quasimodo_models/src/model/ModelMask.cpp +++ b/quasimodo/quasimodo_models/src/model/ModelMask.cpp @@ -9,8 +9,9 @@ namespace reglib int ModelMask_id = 0; -ModelMask::ModelMask(cv::Mat mask_){ +ModelMask::ModelMask(cv::Mat mask_, std::string label_){ mask = mask_; + label = label_; sweepid = -1; id = ModelMask_id++; From 98465a9eacb508dedb556979c3e9bc37e105870f Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 10 Nov 2016 21:56:28 +0100 Subject: [PATCH 208/255] ... --- .../metaroom_additional_view_processing.cpp | 618 ++++++++++-------- 1 file changed, 338 insertions(+), 280 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 60cb193b..b222923d 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -96,6 +96,9 @@ #include +#include +#include + std::vector< ros::ServiceServer > m_DynamicObjectsServiceServers; std::vector< ros::ServiceServer > m_GetDynamicObjectServiceServers; @@ -146,6 +149,7 @@ reglib::Camera * basecam; ros::NodeHandle * np; bool recomputeRelativePoses = false; +bool do_last_and_send = true; void sendMetaroomToServer(std::string path); @@ -158,6 +162,16 @@ void signal_callback_handler(int signum){ exit(signum); } +std::string replaceAll(std::string str, std::string from, std::string to){ + std::size_t found = str.find(from); + while(found!=std::string::npos){ + str.replace(found,from.size(),to); + found = str.find(from); + } + return str; +} + + void remove_old_seg(std::string sweep_folder){ printf("remove_old_seg: %s\n",sweep_folder.c_str()); DIR *dir; @@ -578,81 +592,6 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri } } - // { - // reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); - // std::vector frames; - // std::vector masks; - // std::vector unrefined; - - // std::vector both_unrefined; - // //both_unrefined.push_back(Eigen::Matrix4d::Identity()); - - // std::vector times; - // for(unsigned int i = 0; i < 3000 && i < viewrgbs.size(); i++){ - // printf("additional view: %i\n",i); - // geometry_msgs::TransformStamped msg; - // tf::transformStampedTFToMsg(viewtfs[i], msg); - // long sec = msg.header.stamp.sec; - // long nsec = msg.header.stamp.nsec; - // double time = double(sec)+1e-9*double(nsec); - - // Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]); - - // unrefined.push_back(m); - // times.push_back(time); - - // cv::Mat fullmask; - // fullmask.create(480,640,CV_8UC1); - // unsigned char * maskdata = (unsigned char *)fullmask.data; - // for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} - // masks.push_back(new reglib::ModelMask(fullmask)); - - // reglib::Camera * cam = sweep->frames.front()->camera->clone(); - // reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m,true,savePath);//a.matrix()); - // frames.push_back(frame); - - // both_unrefined.push_back(quasimodo_brain::getMat(viewtfs[0]).inverse()*m); - // } - - // if(frames.size() > 0){ - // reglib::Model * fullmodel = new reglib::Model(); - // fullmodel->points.clear(); - // reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.25); - // if(savePath.size() != 0){ - // bgmassreg->savePath = savePath+"/processAV_"+std::to_string(fullmodel->id); - // } - - // bgmassreg->timeout = 300; - // bgmassreg->viewer = viewer; - // bgmassreg->use_surface = true; - // bgmassreg->use_depthedge = false; - // bgmassreg->visualizationLvl = visualization_lvl_regref; - - // bgmassreg->maskstep = 2; - // bgmassreg->nomaskstep = 2; - - // bgmassreg->nomask = true; - // bgmassreg->stopval = 0.0005; - // //bgmassreg->addModel(sweep); - // bgmassreg->setData(frames,masks); - - // reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); - // delete bgmassreg; - - // for(unsigned int i = 0; i < frames.size(); i++){ - // fullmodel->frames.push_back(frames[i]); - // fullmodel->modelmasks.push_back(masks[i]); - // fullmodel->relativeposes.push_back(bgmfr.poses[i]); - // } - // fullmodel->recomputeModelPoints(); - - // if(savePath.size() != 0){ - // pcl::PointCloud::Ptr cld = fullmodel->getPCLnormalcloud(1, false); - // pcl::io::savePCDFileBinaryCompressed (savePath+"/processAV_"+std::to_string(fullmodel->id)+"_fused.pcd", *cld); - // } - // } - // } - reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); if(sweep->frames.size() == 0){ printf("no frames in sweep, returning\n"); @@ -758,12 +697,8 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri bgmassreg->use_surface = true; bgmassreg->use_depthedge = false; bgmassreg->visualizationLvl = visualization_lvl_regref; - // bgmassreg->maskstep = 5; - // bgmassreg->nomaskstep = 5; - bgmassreg->maskstep = 5; bgmassreg->nomaskstep = 5; - bgmassreg->nomask = true; bgmassreg->stopval = 0.0005; bgmassreg->addModel(sweep); @@ -771,28 +706,8 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); - //printf("%s\n",bgmassreg->savePath.c_str()); - //exit(0); - delete bgmassreg; - // reglib::MassRegistrationPPR2 * bgmassreg2 = new reglib::MassRegistrationPPR2(0.01); - - // bgmassreg2->timeout = 300; - // bgmassreg2->viewer = viewer; - // bgmassreg2->use_surface = true; - // bgmassreg2->use_depthedge = false; - // bgmassreg2->visualizationLvl = visualization_lvl; - // bgmassreg2->maskstep = 2; - // bgmassreg2->nomaskstep = 2; - // bgmassreg2->nomask = true; - // bgmassreg2->stopval = 0.0005; - // bgmassreg2->addModel(sweep); - // bgmassreg2->setData(frames,masks); - - // reglib::MassFusionResults bgmfr2 = bgmassreg2->getTransforms(bgmfr.poses); - // delete bgmassreg2; - for(unsigned int i = 0; i < frames.size(); i++){frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1];} for(unsigned int i = 0; i < frames.size(); i++){ @@ -815,7 +730,6 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri delete reg; delete mu; delete sweep; - //delete basecam; return fullmodel; } @@ -865,8 +779,7 @@ std::vector readPoseXML(std::string xmlFile){ if (token == QXmlStreamReader::StartDocument) continue; - if (xmlReader->hasError()) - { + if (xmlReader->hasError()){ ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); return poses; } @@ -900,7 +813,6 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: int viewgroup_nrviews = readNumberOfViews(sweep_folder+"ViewGroup.xml"); - printf("sweep_folder: %s\n",sweep_folder.c_str()); int additional_nrviews = 0; @@ -948,16 +860,7 @@ int totalcounter = 0; int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = true){ - // CloudPtr denoisedcloud (new Cloud()); - // CloudPtr fullcloud (new Cloud()); - - - - // denoisedcloud->width = 0; - // denoisedcloud->height = 1; - - // fullcloud->width = 0; - // fullcloud->height = 1; + path = replaceAll(path, "//", "/"); quasimodo_brain::cleanPath(path); int returnval = 0; @@ -966,35 +869,16 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr int slash_pos = path.find_last_of("/"); std::string sweep_folder = path.substr(0, slash_pos) + "/"; - - // pcl::io::savePCDFileBinaryCompressed(std::string(buf),*fullcloud); - if ( ! boost::filesystem::exists( path ) ){return 0;} - - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); store_old_xml = objectFiles.size() == 0; - printf("current_roomData starting to load\n"); - SimpleXMLParser parser; SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path,std::vector{"RoomIntermediateCloud","IntermediatePosition"}); - printf("current_roomData loaded\n"); - reglib::Model * fullmodel = getAVMetaroom(path,true,saveVisuals); - printf("fullmodel->frames.size() = %i\n",fullmodel->frames.size()); - - - - - // pcl::io::savePCDFileBinaryCompressed(sweep_folder+"/dynamic_clusters.pcd",*dyncloud); - // pcl::io::savePCDFileBinaryCompressed(sweep_folder+"/denoised_cloud.pcd",*denoisedcloud); - // pcl::io::savePCDFileBinaryCompressed(sweep_folder+"/full_cloud.pcd",*fullcloud); - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( fullmodel, reg); mu->occlusion_penalty = 15; @@ -1012,15 +896,6 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr delete fullmodel; return 0; } - // fullmodel->points = mu->getSuperPoints(po,fr,mm,1,false); - // fullmodel->recomputeModelPoints(); - // SemanticRoom observation = SemanticRoomXMLParser::loadRoomFromXML(path,false); - // Eigen::Matrix4f roomTransform = observation.getRoomTransform(); - // printf("roomTransform\n"); - // std::cout << roomTransform << std::endl; - - // return 0; - DynamicObjectXMLParser objectparser(sweep_folder, true); @@ -1030,11 +905,10 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr int prevind = -1; std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); - //printf("sweep_xmls: %i\n",sweep_xmls.size()); for (unsigned int i = 0; i < sweep_xmls.size(); i++){ + sweep_xmls[i] = replaceAll(sweep_xmls[i], "//", "/"); SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); std::string other_waypointid = other_roomData.roomWaypointId; - //printf("%i: %s\n",i,sweep_xmls[i].c_str()); if(sweep_xmls[i].compare(path) == 0){break;} if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} @@ -1046,7 +920,6 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr int nextind = sweep_xmls.size(); for (int i = int(sweep_xmls.size()-1); i >= 0 ; i--){ if(i < 0){break;} - //printf("%i / %i \n",i,sweep_xmls.size()); SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); std::string other_waypointid = other_roomData.roomWaypointId; @@ -1077,13 +950,6 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr printf("no previous...\n"); } - // if(nextind < sweep_xmls.size()){ - // std::string next = sweep_xmls[nextind]; - // printf("next: %s\n",next.c_str()); - // reglib::Model * nxt = getAVMetaroom(next); - // bgs.push_back(nxt); - // } - printf("bgs.size() = %i\n",bgs.size()); if(bgs.size() > 0){ auto sweep = SimpleXMLParser::loadRoomFromXML(path, std::vector{},false); @@ -1427,12 +1293,6 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr pcl::io::savePCDFileBinaryCompressed(sweep_folder+"/dynamic_clusters.pcd",*dyncloud); } - - - // pcl::io::savePCDFileBinaryCompressed(sweep_folder+"/dynamic_clusters.pcd",*dyncloud); - // pcl::io::savePCDFileBinaryCompressed(sweep_folder+"/denoised_cloud.pcd",*denoisedcloud); - // pcl::io::savePCDFileBinaryCompressed(sweep_folder+"/full_cloud.pcd",*fullcloud); - for(unsigned int i = 0; i < bgs.size(); i++){ bgs[i]->fullDelete(); delete bgs[i]; @@ -1718,16 +1578,11 @@ bool segmentRaresFiles(std::string path, bool resegment){ for (auto sweep_xml : sweep_xmls) { printf("sweep_xml: %s\n",sweep_xml.c_str()); - //processSweepForDatabase(sweep_xml,""); - - quasimodo_brain::cleanPath(sweep_xml); int slash_pos = sweep_xml.find_last_of("/"); std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; QStringList segoutput = QDir(sweep_folder.c_str()).entryList(QStringList("segoutput.txt")); - //QStringList movFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*moving_obj*.xml")); - //printf("sweep_folder: %s\n",sweep_folder.c_str()); printf("segoutput %i\n",segoutput.size()); if(resegment || segoutput.size() == 0){ @@ -1752,11 +1607,12 @@ bool testDynamicObjectServiceCallback(std::string path){ return dynamicObjectsServiceCallback(req,res); } -void roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& obs_msg) { - std::cout<<"Room obs message received"<xml_file_name); + int ret = processMetaroom(dyncloud,path); std_msgs::String msg; if(ret == 0){ @@ -1774,9 +1630,150 @@ void roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& o roomObservationCallback_pubs.publish(input); for(unsigned int i = 0; i < m_PublisherStatuss.size(); i++){m_PublisherStatuss[i].publish(msg);} + + //Send the previous room to the modelserver... + path = replaceAll(path, "//", "/"); + int prevind = -1; + std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + for (unsigned int i = 0; i < sweep_xmls.size(); i++){ + sweep_xmls[i] = replaceAll(sweep_xmls[i], "//", "/"); + if(sweep_xmls[i].compare(path) == 0){break;} + prevind = i; + } + if(prevind >= 0){//Submit last metaroom results + sendMetaroomToServer(sweep_xmls[prevind]); + } +} + +void roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& obs_msg) { + + std::cout<<"Room obs message received"<xml_file_name,""); + //processMetaroom +} + +std::vector getRoomSuperPoints(std::string path, std::string savePath){ + std::vector spvec; + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + std::streampos size; + char * memblock; + + std::ifstream file (sweep_folder+"/superpoints.bin", ios::in|ios::binary|ios::ate); + if (file.is_open()) { + size = file.tellg(); + memblock = new char [size]; + file.seekg (0, ios::beg); + file.read (memblock, size); + file.close(); + + cout << "the entire file content is in memory"; + + long nr_points = size / (sizeof(float)*12); + printf("loading %i points\n",nr_points); + + float * data = (float *)memblock; + + spvec.resize(nr_points); + long count = 0; + for(unsigned long i = 0; i < nr_points; i++){ + reglib::superpoint & p = spvec[i]; + p.point(0) = data[count++]; + p.point(1) = data[count++]; + p.point(2) = data[count++]; + p.normal(0) = data[count++]; + p.normal(1) = data[count++]; + p.normal(2) = data[count++]; + p.feature(0) = data[count++]; + p.feature(1) = data[count++]; + p.feature(2) = data[count++]; + p.point_information = data[count++]; + p.normal_information = data[count++]; + p.feature_information = data[count++]; + } + + delete[] memblock; + }else{ + std::cout << "Unable to open file superpoint file, go process that data" << std::endl; + processSweepForDatabase(sweep_folder+"/room.xml",savePath); + } + + return spvec; +} + +void transformSuperPoints(std::vector & spvec, Eigen::Matrix4d cp){ + float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); + float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); + float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); + + for(unsigned long i = 0; i < spvec.size(); i++){ + reglib::superpoint & p = spvec[i]; + + float x = p.point(0); + float y = p.point(1); + float z = p.point(2); + float nx = p.normal(0); + float ny = p.normal(1); + float nz = p.normal(2); + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + float tnx = m00*nx + m01*ny + m02*nz; + float tny = m10*nx + m11*ny + m12*nz; + float tnz = m20*nx + m21*ny + m22*nz; + + p.point(0) = tx; + p.point(1) = ty; + p.point(2) = tz; + p.normal(0) = tnx; + p.normal(1) = tny; + p.normal(2) = tnz; + } +} + +void saveSuperPoints(std::string path, std::vector & spvec, Eigen::Matrix4d pose, float ratio_keep = 0.1){ + printf("saveSuperPoints(%s)\n",path.c_str()); + transformSuperPoints(spvec,pose); + //XYZ RGB NXNYNZ W1 W2 + long sizeofSuperPoint = 3*(3+1); + + std::ofstream file; + file.open(path, ios::out | ios::binary); + if(spvec.size() != 0){ + float * data = new float[spvec.size() * sizeofSuperPoint]; + long added = 0; + long count = 0; + for(unsigned long i = 0; i < spvec.size(); i++){ + if(double(rand() % 1000)*0.001 > ratio_keep ){continue;} + reglib::superpoint & p = spvec[i]; + data[count++] = p.point(0); + data[count++] = p.point(1); + data[count++] = p.point(2); + data[count++] = p.normal(0); + data[count++] = p.normal(1); + data[count++] = p.normal(2); + data[count++] = p.feature(0); + data[count++] = p.feature(1); + data[count++] = p.feature(2); + data[count++] = p.point_information; + data[count++] = p.normal_information; + data[count++] = p.feature_information; + added++; + } + if(added > 0){ + printf("saving %i points\n",added); + file.write( (char*)data, added*sizeofSuperPoint*sizeof(float)); + } + delete[] data; + } + file.close(); } void processSweepForDatabase(std::string path, std::string savePath){ + path = replaceAll(path, "//", "/"); printf("processSweepForDatabase(%s)\n",path.c_str()); if ( ! boost::filesystem::exists( path ) ){return;} @@ -1784,114 +1781,170 @@ void processSweepForDatabase(std::string path, std::string savePath){ std::string sweep_folder = path.substr(0, slash_pos) + "/"; printf("sweep_folder: %s\n",sweep_folder.c_str()); + SimpleXMLParser parser; + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml",std::vector(),false,false); + std::string current_waypointid = roomData.roomWaypointId; -// ros::ServiceClient client = n->serviceClient("/soma_llsd/insert_scene"); -// ROS_INFO("Waiting for /soma_llsd/insert_scene service..."); -// if (!client.waitForExistence(ros::Duration(1.0))) { -// ROS_INFO("Failed to get /soma_llsd/insert_scene service!"); -// return; -// } -// ROS_INFO("Got /soma_llsd/insert_scene service"); - -// segment.id = std::to_string(model.model_id); + //Send the previous room to the modelserver... + int firstind = -1; + int prevind = -1; + std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + for (unsigned int i = 0; i < sweep_xmls.size(); i++){ + sweep_xmls[i] = replaceAll(sweep_xmls[i], "//", "/"); + SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); -// size_t counter = 0; -// for (const quasimodo_msgs::rgbd_frame& frame : model.frames) { -// soma_llsd::InsertScene scene; -// scene.request.rgb_img = frame.rgb; -// scene.request.depth_img = frame.depth; -// scene.request.camera_info = frame.camera; -// scene.request.robot_pose = frame.pose; // not good actually but whatevs -// scene.request.cloud = model.clouds[counter]; + if(sweep_xmls[i].compare(path) == 0){break;} + std::string other_waypointid = other_roomData.roomWaypointId; + if(firstind == -1 && other_waypointid.compare(current_waypointid) == 0){firstind = i;} + if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} -// if (!client.call(scene)) { -// ROS_ERROR("Failed to call service /soma_llsd/insert_scene"); -// return; -// } + } + printf("%i %i\n",prevind,firstind); + + Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); + if(firstind != -1){ + processSweepForDatabase(sweep_xmls[firstind],savePath); + printf("first: %s\n",sweep_xmls[firstind].c_str()); + std::vector backgroundsp = getRoomSuperPoints(sweep_xmls[firstind],savePath); + if(prevind != -1 && prevind != firstind){ + printf("prev: %s\n",sweep_xmls[prevind].c_str()); + std::vector prev_vec = getRoomSuperPoints(sweep_xmls[prevind],savePath); + backgroundsp.insert(backgroundsp.end(), prev_vec.begin(), prev_vec.end()); + } + Eigen::Matrix4d currentpose = Eigen::Matrix4d::Identity(); + roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); + if(roomData.vIntermediateRoomCloudTransforms.size() != 0){ + currentpose = quasimodo_brain::getMat(roomData.vIntermediateRoomCloudTransforms[0]); + } + Eigen::Matrix4d firstpose = Eigen::Matrix4d::Identity(); + SimpleXMLParser::RoomData first_roomData = parser.loadRoomFromXML(sweep_xmls[firstind]); + if(first_roomData.vIntermediateRoomCloudTransforms.size() != 0){ + firstpose = quasimodo_brain::getMat(first_roomData.vIntermediateRoomCloudTransforms[0]); + } + exit(0); + }else{//this is the first room, set to whatever odometry is + printf("this is the first room, set to whatever odometry is!\n"); + roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); + if(roomData.vIntermediateRoomCloudTransforms.size() != 0){ + pose = quasimodo_brain::getMat(roomData.vIntermediateRoomCloudTransforms[0]); + } + } + std::cout << pose << std::endl << std::endl; reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); - for(unsigned int i = 0; i < sweep->frames.size(); i++){ - reglib::RGBDFrame * frame = sweep->frames[i]; - geometry_msgs::Pose pose; - tf::poseEigenToMsg (Eigen::Affine3d(frame->pose), pose); - - cv_bridge::CvImage rgbBridgeImage; - rgbBridgeImage.image = frame->rgb; - rgbBridgeImage.encoding = "bgr8"; - cv_bridge::CvImage depthBridgeImage; - depthBridgeImage.image = frame->depth; - depthBridgeImage.encoding = "mono16"; -// cv_bridge::CvImage maskBridgeImage; -// maskBridgeImage.image = model->modelmasks[i]->getMask(); -// maskBridgeImage.encoding = "mono8"; -// msg.local_poses[startsize+i] = pose1; -// msg.frames[startsize+i].capture_time = ros::Time(); -// msg.frames[startsize+i].pose = pose2; -// msg.frames[startsize+i].frame_id = model->frames[i]->id; -// msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); -// msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); -// msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg()); - -// msg.frames[startsize+i].camera.K[0] = model->frames[i]->camera->fx; -// msg.frames[startsize+i].camera.K[4] = model->frames[i]->camera->fy; -// msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx; -// msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy; - -// sensor_msgs::PointCloud2 output; -// if(addClouds){pcl::toROSMsg(*(model->frames[i]->getPCLcloud()), output);} -// msg.clouds.push_back(output); - } + saveSuperPoints(sweep_folder+"/superpoints.bin",sweep->points,pose); +// std::vector testvec = getRoomSuperPoints(sweep_folder+"/superpoints.bin",savePath); + + + exit(0); + roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); + + + // ros::ServiceClient client = n->serviceClient("/soma_llsd/insert_scene"); + // ROS_INFO("Waiting for /soma_llsd/insert_scene service..."); + // if (!client.waitForExistence(ros::Duration(1.0))) { + // ROS_INFO("Failed to get /soma_llsd/insert_scene service!"); + // return; + // } + // ROS_INFO("Got /soma_llsd/insert_scene service"); + + // segment.id = std::to_string(model.model_id); + + // size_t counter = 0; + // for (const quasimodo_msgs::rgbd_frame& frame : model.frames) { + + // if (!client.call(scene)) { + // ROS_ERROR("Failed to call service /soma_llsd/insert_scene"); + // return; + // } -// int startsize = msg.local_poses.size(); -// msg.local_poses.resize(startsize+model->relativeposes.size()); -// msg.frames.resize(startsize+model->frames.size()); -// msg.masks.resize(startsize+model->modelmasks.size()); -// for(unsigned int i = 0; i < model->relativeposes.size(); i++){ -// geometry_msgs::Pose pose1; -// tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i])*rp, pose1); -// geometry_msgs::Pose pose2; -// tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose)*rp, pose2); -// cv_bridge::CvImage rgbBridgeImage; -// rgbBridgeImage.image = model->frames[i]->rgb; -// rgbBridgeImage.encoding = "bgr8"; -// cv_bridge::CvImage depthBridgeImage; -// depthBridgeImage.image = model->frames[i]->depth; -// depthBridgeImage.encoding = "mono16"; -// cv_bridge::CvImage maskBridgeImage; -// maskBridgeImage.image = model->modelmasks[i]->getMask(); -// maskBridgeImage.encoding = "mono8"; -// msg.local_poses[startsize+i] = pose1; -// msg.frames[startsize+i].capture_time = ros::Time(); -// msg.frames[startsize+i].pose = pose2; -// msg.frames[startsize+i].frame_id = model->frames[i]->id; -// msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); -// msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); -// msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg()); - -// msg.frames[startsize+i].camera.K[0] = model->frames[i]->camera->fx; -// msg.frames[startsize+i].camera.K[4] = model->frames[i]->camera->fy; -// msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx; -// msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy; - -// sensor_msgs::PointCloud2 output; -// if(addClouds){pcl::toROSMsg(*(model->frames[i]->getPCLcloud()), output);} -// msg.clouds.push_back(output); -// } -// for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ -// addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp,addClouds); -// } + + + + + //Get first sweep and previous sweep, merge those + //Get first sweep and previous sweep, merge those + + + + // for(unsigned int i = 0; i < sweep->frames.size(); i++){ + // reglib::RGBDFrame * frame = sweep->frames[i]; + // geometry_msgs::Pose pose; + // tf::poseEigenToMsg (Eigen::Affine3d(frame->pose), pose); + + // cv_bridge::CvImage rgbBridgeImage; + // rgbBridgeImage.image = frame->rgb; + // rgbBridgeImage.encoding = "bgr8"; + // cv_bridge::CvImage depthBridgeImage; + // depthBridgeImage.image = frame->depth; + // depthBridgeImage.encoding = "mono16"; + + // soma_llsd::InsertScene scene; + // scene.request.rgb_img = *(rgbBridgeImage.toImageMsg()); + // scene.request.depth_img = *(depthBridgeImage.toImageMsg()); + // scene.request.camera_info.K[0] = frame->camera->fx; + // scene.request.camera_info.K[4] = frame->camera->fy; + // scene.request.camera_info.K[2] = frame->camera->cx; + // scene.request.camera_info.K[5] = frame->camera->cy; + // //scene.request.robot_pose = frame.pose; // not good actually but whatevs + + // sensor_msgs::PointCloud2 input; + // pcl::toROSMsg (*(roomData.vIntermediateRoomClouds[i]),input);//, *transformed_cloud); + // input.header.frame_id = "/map"; + // scene.request.cloud = input; + // } + + + // int startsize = msg.local_poses.size(); + // msg.local_poses.resize(startsize+model->relativeposes.size()); + // msg.frames.resize(startsize+model->frames.size()); + // msg.masks.resize(startsize+model->modelmasks.size()); + // for(unsigned int i = 0; i < model->relativeposes.size(); i++){ + // geometry_msgs::Pose pose1; + // tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i])*rp, pose1); + // geometry_msgs::Pose pose2; + // tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose)*rp, pose2); + // cv_bridge::CvImage rgbBridgeImage; + // rgbBridgeImage.image = model->frames[i]->rgb; + // rgbBridgeImage.encoding = "bgr8"; + // cv_bridge::CvImage depthBridgeImage; + // depthBridgeImage.image = model->frames[i]->depth; + // depthBridgeImage.encoding = "mono16"; + // cv_bridge::CvImage maskBridgeImage; + // maskBridgeImage.image = model->modelmasks[i]->getMask(); + // maskBridgeImage.encoding = "mono8"; + // msg.local_poses[startsize+i] = pose1; + // msg.frames[startsize+i].capture_time = ros::Time(); + // msg.frames[startsize+i].pose = pose2; + // msg.frames[startsize+i].frame_id = model->frames[i]->id; + // msg.frames[startsize+i].rgb = *(rgbBridgeImage.toImageMsg()); + // msg.frames[startsize+i].depth = *(depthBridgeImage.toImageMsg()); + // msg.masks[startsize+i] = *(maskBridgeImage.toImageMsg()); + + // msg.frames[startsize+i].camera.K[0] = model->frames[i]->camera->fx; + // msg.frames[startsize+i].camera.K[4] = model->frames[i]->camera->fy; + // msg.frames[startsize+i].camera.K[2] = model->frames[i]->camera->cx; + // msg.frames[startsize+i].camera.K[5] = model->frames[i]->camera->cy; + + // sensor_msgs::PointCloud2 output; + // if(addClouds){pcl::toROSMsg(*(model->frames[i]->getPCLcloud()), output);} + // msg.clouds.push_back(output); + // } + // for(unsigned int i = 0; i < model->submodels_relativeposes.size(); i++){ + // addToModelMSG(msg,model->submodels[i],Eigen::Affine3d(model->submodels_relativeposes[i])*rp,addClouds); + // } sweep->fullDelete(); @@ -1951,6 +2004,7 @@ bool testPath(std::string path){ vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(path); for (auto sweep_xml : sweep_xmls) { printf("sweep_xml: %s\n",sweep_xml.c_str()); + //processSweep(sweep_xml,""); processSweepForDatabase(sweep_xml,""); } return false; @@ -2004,31 +2058,31 @@ int main(int argc, char** argv){ int inputstate = 0; for(int i = 1; i < argc;i++){ printf("input: %s\n",argv[i]); - if( std::string(argv[i]).compare("-intopic") == 0){ inputstate = 0;} - else if(std::string(argv[i]).compare("-outtopic") == 0){ inputstate = 1;} - else if(std::string(argv[i]).compare("-file") == 0){ inputstate = 2;} - else if(std::string(argv[i]).compare("-v") == 0){ visualization_lvl = 1;inputstate = 3;} - else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 4;} - else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} - else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} - else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} - else if(std::string(argv[i]).compare("-sendModel") == 0){ inputstate = 8;} - else if(std::string(argv[i]).compare("-sendSub") == 0) { inputstate = 9;} - else if(std::string(argv[i]).compare("-sendTopic") == 0){ inputstate = 10;} - else if(std::string(argv[i]).compare("-roomObservationTopic") == 0){ inputstate = 11;} - else if(std::string(argv[i]).compare("-DynamicObjectsService") == 0){ inputstate = 12;} - else if(std::string(argv[i]).compare("-GetDynamicObjectService") == 0){ inputstate = 13;} - else if(std::string(argv[i]).compare("-statusmsg") == 0){ inputstate = 14;} - else if(std::string(argv[i]).compare("-files") == 0){ inputstate = 15;} - else if(std::string(argv[i]).compare("-baseSweep") == 0){ inputstate = 16;} - else if(std::string(argv[i]).compare("-resegment") == 0){ resegment = true;} - else if(std::string(argv[i]).compare("-once") == 0){ once = true;} - else if(std::string(argv[i]).compare("-nobase") == 0){ baseSetting = false;} - else if(std::string(argv[i]).compare("-recomputeRelativePoses") == 0){ recomputeRelativePoses = true;} + if( std::string(argv[i]).compare("-intopic") == 0){ inputstate = 0;} + else if(std::string(argv[i]).compare("-outtopic") == 0){ inputstate = 1;} + else if(std::string(argv[i]).compare("-file") == 0){ inputstate = 2;} + else if(std::string(argv[i]).compare("-v") == 0){ visualization_lvl = 1; inputstate = 3;} + else if(std::string(argv[i]).compare("-folder") == 0){ inputstate = 4;} + else if(std::string(argv[i]).compare("-train") == 0){ inputstate = 5;} + else if(std::string(argv[i]).compare("-posepath") == 0){ inputstate = 6;} + else if(std::string(argv[i]).compare("-loadposes") == 0){ inputstate = 7;} + else if(std::string(argv[i]).compare("-sendModel") == 0){ inputstate = 8;} + else if(std::string(argv[i]).compare("-sendSub") == 0) { inputstate = 9;} + else if(std::string(argv[i]).compare("-sendTopic") == 0){ inputstate = 10;} + else if(std::string(argv[i]).compare("-roomObservationTopic") == 0){ inputstate = 11;} + else if(std::string(argv[i]).compare("-DynamicObjectsService") == 0){ inputstate = 12;} + else if(std::string(argv[i]).compare("-GetDynamicObjectService") == 0){ inputstate = 13;} + else if(std::string(argv[i]).compare("-statusmsg") == 0){ inputstate = 14;} + else if(std::string(argv[i]).compare("-files") == 0){ inputstate = 15;} + else if(std::string(argv[i]).compare("-baseSweep") == 0){ inputstate = 16;} + else if(std::string(argv[i]).compare("-resegment") == 0){ resegment = true;} + else if(std::string(argv[i]).compare("-once") == 0){ once = true;} + else if(std::string(argv[i]).compare("-nobase") == 0){ baseSetting = false;} + else if(std::string(argv[i]).compare("-recomputeRelativePoses") == 0){ recomputeRelativePoses = true;} else if(std::string(argv[i]).compare("-v_init") == 0){ visualization_lvl_regini = 1;inputstate = 17;} else if(std::string(argv[i]).compare("-v_reg") == 0){ visualization_lvl_regref = 1;inputstate = 18;} - else if(std::string(argv[i]).compare("-saveVisuals") == 0){ inputstate = 19;} - else if(std::string(argv[i]).compare("-testpaths") == 0){ inputstate = 20;} + else if(std::string(argv[i]).compare("-saveVisuals") == 0){ inputstate = 19;} + else if(std::string(argv[i]).compare("-testpaths") == 0){ inputstate = 20;} else if(inputstate == 0){ segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); }else if(inputstate == 1){ @@ -2072,6 +2126,7 @@ int main(int argc, char** argv){ saveVisuals = std::string(argv[i]); }else if(inputstate == 20){ testpaths.push_back(std::string(argv[i])); + printf("testpaths: %i\n",testpaths.size()); } } @@ -2109,7 +2164,7 @@ int main(int argc, char** argv){ } if(processAndSendsubs.size() == 0){ - processAndSendsubs.push_back(n.subscribe("/object_learning/learned_object_xml", 1000, processAndSendCallback)); + processAndSendsubs.push_back(n.subscribe("/object_learning/learned_object_xml", 1000, chatterCallback));//processAndSendCallback)); } } @@ -2118,7 +2173,10 @@ int main(int argc, char** argv){ printf("overall_folder: %s\n",overall_folder.c_str()); - for(unsigned int i = 0; i < testpaths.size(); i++){testPath(testpaths[i]);} + printf("testpaths: %i\n",testpaths.size()); + for(unsigned int i = 0; i < testpaths.size(); i++){ + testPath(testpaths[i]); + } for(unsigned int i = 0; i < raresfiles.size(); i++){segmentRaresFiles( raresfiles[i], raresfiles_resegment[i]);} for(unsigned int i = 0; i < trainMetarooms.size(); i++){ trainMetaroom( trainMetarooms[i]);} for(unsigned int i = 0; i < processMetarooms.size(); i++){ From 31331a75aac77969ac5dc094da5d119709b963f8 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 14 Nov 2016 15:13:27 +0100 Subject: [PATCH 209/255] Fixed the last soma2 include --- .../quasimodo_retrieval/src/quasimodo_logging_server.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp index 54af76b7..1e57f419 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_logging_server.cpp @@ -16,7 +16,7 @@ // for logging to soma in mongodb #include -#include +#include #include using namespace std; @@ -40,7 +40,7 @@ class logging_server { void callback(const quasimodo_msgs::retrieval_query_result& res) { - soma_manager::SOMA2InsertObjs::Request soma_req; + soma_manager::SOMAInsertObjs::Request soma_req; size_t N = res.result.retrieved_clouds.size(); soma_req.objects.resize(N); From 390371b002d881322623028efda894eb91eecf03 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 15 Nov 2016 22:14:07 +0100 Subject: [PATCH 210/255] changes to metaroom calib --- .../src/calibrate_sweep_action_server.cpp | 227 ++- semantic_map/include/semantic_map/room.h | 1 + semantic_map/include/semantic_map/room.hpp | 32 + .../include/semantic_map/room_xml_parser.hpp | 49 +- .../strands_sweep_registration/Camera.h | 6 + .../strands_sweep_registration/Frame.h | 1 + .../ProblemFrameConnection.h | 4 +- .../RobotContainer.h | 10 + strands_sweep_registration/src/Camera.cpp | 26 +- strands_sweep_registration/src/Frame.cpp | 8 + .../src/ProblemFrameConnection.cpp | 15 +- .../src/RobotContainer.cpp | 1306 ++++++++++------- 12 files changed, 1098 insertions(+), 587 deletions(-) diff --git a/calibrate_sweeps/src/calibrate_sweep_action_server.cpp b/calibrate_sweeps/src/calibrate_sweep_action_server.cpp index 76f58dfa..18f96b41 100644 --- a/calibrate_sweeps/src/calibrate_sweep_action_server.cpp +++ b/calibrate_sweeps/src/calibrate_sweep_action_server.cpp @@ -26,8 +26,8 @@ using namespace std; typedef actionlib::SimpleActionServer Server; -std::string runCalibration(int min_num_sweeps = 1, int max_num_sweeps = 100000, std::string sweep_location = "", std::string save_location = ""){ - +std::string runCalibration(int min_num_sweeps = 1, int max_num_sweeps = 100000, std::string sweep_location = "", std::string save_location = "", + bool newParams = true, float fx = 534.191590, float fy = 534.016892, float cx = 315.622746, float cy = 238.568515){ if (sweep_location=="") { passwd* pw = getpwuid(getuid()); @@ -45,8 +45,23 @@ std::string runCalibration(int min_num_sweeps = 1, int max_num_sweeps = 100000, return ""; } + //SweepParameters complete_sweep_parameters (-160, 20, 160, -30, 30, 30); + SweepParameters complete_sweep_parameters (-160, 20, 160, -30, 30, 30); + + SweepParameters medium_sweep_parameters (-160, 20, 160, -30, -30, -30); + SweepParameters short_sweep_parameters (-160, 40, 160, -30, -30, -30); + SweepParameters shortest_sweep_parameters (-160, 60, 140, -30, -30, -30); + + SweepParameters medium_sweep_parameters2 (-160, 20, 160, -30, 30, -30); + SweepParameters short_sweep_parameters2 (-160, 40, 160, -30, 30, -30); + SweepParameters shortest_sweep_parameters2 (-160, 60, 140, -30, 30, -30); + + ROS_INFO_STREAM("complete_sweep_parameters paramters " << complete_sweep_parameters); + ROS_INFO_STREAM("medium_sweep_parameters paramters " << medium_sweep_parameters); + ROS_INFO_STREAM("short_sweep_parameters paramters " << short_sweep_parameters); + ROS_INFO_STREAM("shortest_sweep_parameters paramters " << shortest_sweep_parameters); + - SweepParameters complete_sweep_parameters (-160, 20, 160, -30, 30, 30); ROS_INFO_STREAM("Calibrating using sweeps with paramters "<initializeCamera(534.191590, 534.016892,315.622746, 238.568515, 640, 480); -//exit(0); - // initialize camera parameters from the sweep - /* - if (matchingObservations.size()){ - SemanticRoom aRoom = SemanticRoomXMLParser::loadRoomFromXML(matchingObservations[0],true); - if (aRoom.getIntermediateCloudCameraParameters().size()){ - image_geometry::PinholeCameraModel aCameraModel = aRoom.getIntermediateCloudCameraParameters()[0]; - rc->initializeCamera(aCameraModel.fx(), aCameraModel.fy(), aCameraModel.cx(), aCameraModel.cy(), aCameraModel.fullResolution().width, aCameraModel.fullResolution().height); - } else { - // no camera parameters saved with the sweep -> initialize optimizer with default parameters - rc->initializeCamera(540.0, 540.0,319.5, 219.5, 640, 480); +//printf("%f %f %f %f\n",534.191590, 534.016892,315.622746, 238.568515); +// rc->initializeCamera(534.191590, 534.016892,315.622746, 238.568515, 640, 480); + + + + if(newParams){ + //printf("%f %f %f %f\n",fx,fy,cx,cy); + rc->initializeCamera(fx,fy,cx,cy, 640, 480); + }else{ + // initialize camera parameters from the sweep + if (matchingObservations.size()){ + SemanticRoom aRoom = SemanticRoomXMLParser::loadRoomFromXML(matchingObservations.front(),true); + if (aRoom.getIntermediateCloudCameraParameters().size()){ + image_geometry::PinholeCameraModel aCameraModel = aRoom.getIntermediateCloudCameraParameters()[0]; + rc->initializeCamera(aCameraModel.fx(), aCameraModel.fy(), aCameraModel.cx(), aCameraModel.cy(), aCameraModel.fullResolution().width, aCameraModel.fullResolution().height); + } else { + // no camera parameters saved with the sweep -> initialize optimizer with default parameters + rc->initializeCamera(540.0, 540.0,319.5, 219.5, 640, 480); + } + }else{ + exit(0); } - }*/ + } + + CloudPtr dummy_cloud (new Cloud()); + dummy_cloud->width = 640; + dummy_cloud->height = 480; + dummy_cloud->points.resize(dummy_cloud->width*dummy_cloud->height); + +//exit(0); +// SweepParameters complete_sweep_parameters (-160, 20, 160, -30, 30, 30); +// SweepParameters medium_sweep_parameters (-160, 20, 160, -30, 30, -30); +// SweepParameters short_sweep_parameters (-160, 40, 160, -30, 30, -30); +// SweepParameters shortest_sweep_parameters (-160, 60, 140, -30, 30, -30); for (size_t i=0; i aRoom = SemanticRoomXMLParser::loadRoomFromXML(matchingObservations[i],true); - if (aRoom.m_SweepParameters != complete_sweep_parameters){ - ROS_INFO_STREAM("Skipping "< features = semantic_map_registration_features::loadRegistrationFeaturesFromSingleSweep(matchingObservations[i], false); - if (features.size() == 0) - { - // recompute orb + unsigned found = matchingObservations[i].find_last_of("/"); + std::string base_path = matchingObservations[i].substr(0,found+1); + RegistrationFeatures reg(false); + //reg.saveOrbFeatures(aRoom,base_path); - unsigned found = matchingObservations[i].find_last_of("/"); - std::string base_path = matchingObservations[i].substr(0,found+1); - RegistrationFeatures reg(false); + std::vector features = semantic_map_registration_features::loadRegistrationFeaturesFromSingleSweep(matchingObservations[i], false); + if (features.size() == 0){ + SemanticRoom aRoom = SemanticRoomXMLParser::loadRoomFromXML(matchingObservations[i],true); reg.saveOrbFeatures(aRoom,base_path); } rc->addToTrainingORBFeatures(matchingObservations[i]); - } +/* + if (aRoom.m_SweepParameters == complete_sweep_parameters){ + printf("complete_sweep_parameters\n"); + + if (features.size() == 0){reg.saveOrbFeatures(aRoom,base_path);} + rc->addToTrainingORBFeatures(matchingObservations[i]); + + }else if (aRoom.m_SweepParameters == medium_sweep_parameters || aRoom.m_SweepParameters == medium_sweep_parameters2 ){ + printf("medium_sweep_parameters\n"); + + for(unsigned int j = 0; j < before.size(); j++){ + after.push_back(before[j]); + } + + while(after.size() < 51){after.push_back(dummy_cloud);} + aRoom.setIntermediateClouds(after); + + if (features.size() == 0){reg.saveOrbFeatures(aRoom,base_path);} + rc->addToTrainingORBFeatures(matchingObservations[i]); + + }else if (aRoom.m_SweepParameters == short_sweep_parameters || aRoom.m_SweepParameters == short_sweep_parameters2 ){ + printf("short_sweep_parameters\n"); + }else if (aRoom.m_SweepParameters == shortest_sweep_parameters || aRoom.m_SweepParameters == shortest_sweep_parameters2 ){ + printf("shortest_sweep_parameters\n"); + }else{ + printf("sweep has no known type\n"); + std::cout <<"sweep parameters not correct: " << aRoom.m_SweepParameters << std::endl << std::endl << std::endl << std::endl; + } +*/ + +// if (aRoom.m_SweepParameters != complete_sweep_parameters){ +// ROS_INFO_STREAM("Skipping "< cameraPoses = rc->train(); + std::vector registeredPoses; for (auto eigenPose : cameraPoses) @@ -172,6 +240,17 @@ printf("%f %f %f %f\n",534.191590, 534.016892,315.622746, 238.568515); registeredPoses = semantic_map_registration_transforms::loadRegistrationTransforms(registeredPosesFile); ROS_INFO_STREAM("Calibration poses saved at: "< complete_registeredPoses = registeredPoses; + + std::vector medium_registeredPoses; + for(unsigned int i = 0; i < 17; i+=1){medium_registeredPoses.push_back(registeredPoses[i]);} + + std::vector short_registeredPoses; + for(unsigned int i = 0; i < 17; i+=2){short_registeredPoses.push_back(registeredPoses[i]);} + + std::vector shortest_registeredPoses; + for(unsigned int i = 0; i < 17; i+=3){shortest_registeredPoses.push_back(registeredPoses[i]);} + std::string sweepParametersFile = semantic_map_registration_transforms::saveSweepParameters(complete_sweep_parameters); ROS_INFO_STREAM("Calibration sweep parameters saved at: "< reg_parser(saveLocation); - for (auto usedObs : matchingObservations) - { + for (auto usedObs : matchingObservations){ SemanticRoom aRoom = SemanticRoomXMLParser::loadRoomFromXML(usedObs,true); auto origTransforms = aRoom.getIntermediateCloudTransforms(); aRoom.clearIntermediateCloudRegisteredTransforms(); @@ -225,16 +305,16 @@ printf("%f %f %f %f\n",534.191590, 534.016892,315.622746, 238.568515); semantic_map_room_utilities::rebuildRegisteredCloud(aRoom); // transform to global frame of reference tf::StampedTransform origin = origTransforms[0]; - CloudPtr completeCloud = aRoom.getCompleteRoomCloud(); - pcl_ros::transformPointCloud(*completeCloud, *completeCloud,origin); - aRoom.setCompleteRoomCloud(completeCloud); - string room_path = reg_parser.saveRoomAsXML(aRoom); +// CloudPtr completeCloud = aRoom.getCompleteRoomCloud(); +// pcl_ros::transformPointCloud(*completeCloud, *completeCloud,origin); +// aRoom.setCompleteRoomCloud(completeCloud); + string room_path = reg_parser.saveRoomAsXML(aRoom,"room.xml",true); ROS_INFO_STREAM("..done"); // recompute ORB features - unsigned found = room_path.find_last_of("/"); - std::string base_path = room_path.substr(0,found+1); - RegistrationFeatures reg(false); - reg.saveOrbFeatures(aRoom,base_path); +// unsigned found = room_path.find_last_of("/"); +// std::string base_path = room_path.substr(0,found+1); +// RegistrationFeatures reg(false); +// reg.saveOrbFeatures(aRoom,base_path); } delete rc; return saveLocation; @@ -317,7 +397,7 @@ void execute(const calibrate_sweeps::CalibrateSweepsGoalConstPtr& goal, Server* if (!boost::filesystem::create_directory(saveLocation)) { ROS_ERROR_STREAM("Could not create folder where to save calibration data "+saveLocation); - as->setAborted(res,"Could not create folder where to save calibration data "+saveLocation); + as->setAborted(res,"Could not create folder where to /media/johane/SSDstorage/final_tsc_semantic_maps/semantic_maps//20160808/patrol_run_110/room_0/save calibration data "+saveLocation); return; } } @@ -484,6 +564,69 @@ int main(int argc, char** argv) ROS_INFO_STREAM("Calibrate sweep action server initialized"); server.start(); ros::Subscriber sub = n.subscribe("calibrate_sweeps_action_server/run", 1000, runCallback); + + bool runCalib = false; + std::vector recalibPaths; + //std::string recalibPath = std::string(getenv ("HOME"))+"/.semanticMap/"; + bool loadParams = false; + float fx = 535; + float fy = 535; + float cx = 0.5*(640.0-1); + float cy = 0.5*(480.0-1); + + int max_sweeps = 100; + + int inputstate = -1; + for(unsigned int i = 1; i < argc; i++){ + if(std::string(argv[i]).compare("-run") == 0){ + runCalib = true; + inputstate = 0; + }else if(std::string(argv[i]).compare("-fx") == 0){ + inputstate = 1; + }else if(std::string(argv[i]).compare("-fy") == 0){ + inputstate = 2; + }else if(std::string(argv[i]).compare("-cx") == 0){ + inputstate = 3; + }else if(std::string(argv[i]).compare("-cy") == 0){ + inputstate = 4; + }else if(std::string(argv[i]).compare("-max_sweeps") == 0){ + inputstate = 5; + }else if(std::string(argv[i]).compare("-loadParams") == 0){ + loadParams = true; + }else if(inputstate == 0){ + recalibPaths.push_back(std::string(argv[i])); + }else if(inputstate == 1){ + loadParams = true; + fx = atof(argv[i]); + }else if(inputstate == 2){ + loadParams = true; + fy = atof(argv[i]); + }else if(inputstate == 3){ + loadParams = true; + cx = atof(argv[i]); + }else if(inputstate == 4){ + loadParams = true; + cy = atof(argv[i]); + }else if(inputstate == 5){ + max_sweeps = atoi(argv[i]); + } + } + + if(runCalib){ + if(recalibPaths.size() == 0){ + recalibPaths.push_back(std::string(getenv ("HOME"))+"/.semanticMap/"); + } + for(unsigned int i = 0; i < recalibPaths.size(); i++){ + if(loadParams){ + runCalibration(1,max_sweeps,recalibPaths[i], "",loadParams,fx,fy,cx,cy); + }else{ + runCalibration(1,max_sweeps,recalibPaths[i], ""); + } + //bool newParams = true, float fx = 534.191590, float fy = 534.016892, float cx = 315.622746, float cy = 238.568515 + } + exit(0); + } + ros::spin(); return 0; } diff --git a/semantic_map/include/semantic_map/room.h b/semantic_map/include/semantic_map/room.h index fd4ed897..7f855362 100644 --- a/semantic_map/include/semantic_map/room.h +++ b/semantic_map/include/semantic_map/room.h @@ -95,6 +95,7 @@ class SemanticRoom : public RoomBase{ bool getSaveIntermediateClouds(); void setSaveIntermediateClouds(bool saveIntermediate); auto getIntermediateClouds() -> decltype (m_vIntermediateRoomClouds); + void setIntermediateClouds(std::vector< CloudPtr > clouds_); void clearIntermediateCloudRegisteredTransforms(); void addIntermediateRoomCloudRegisteredTransform(tf::StampedTransform cloud_reg_tf); diff --git a/semantic_map/include/semantic_map/room.hpp b/semantic_map/include/semantic_map/room.hpp index 6edf8cb3..237f074c 100644 --- a/semantic_map/include/semantic_map/room.hpp +++ b/semantic_map/include/semantic_map/room.hpp @@ -166,6 +166,38 @@ auto SemanticRoom::getIntermediateClouds() -> decltype(m_vIntermediat { return m_vIntermediateRoomClouds; } +/* +typedef pcl::PointCloud Cloud; +typedef typename Cloud::Ptr CloudPtr; + +struct IntermediatePositionImages +{ + std::vector vIntermediateDepthImages; + std::vector vIntermediateRGBImages; + tf::StampedTransform intermediateDepthTransform; + tf::StampedTransform intermediateRGBTransform; + image_geometry::PinholeCameraModel intermediateRGBCamParams; + image_geometry::PinholeCameraModel intermediateDepthCamParams; + int numRGBImages, numDepthImages; + bool images_loaded; +}; + +private: + +CloudPtr m_DynamicClustersCloud; +bool m_DynamicClustersLoaded; +std::string m_DynamicClustersFilename; +// intermediate room clouds +std::vector m_vIntermediateRoomClouds; +*/ + +template +void SemanticRoom::setIntermediateClouds(std::vector< CloudPtr > clouds_) +{ + printf("setIntermediateClouds\n"); + m_vIntermediateRoomClouds = clouds_; +} +//std::vector m_vIntermediateRoomClouds; template void SemanticRoom::addIntermediateRoomCloudRegisteredTransform(tf::StampedTransform cloud_reg_tf) diff --git a/semantic_map/include/semantic_map/room_xml_parser.hpp b/semantic_map/include/semantic_map/room_xml_parser.hpp index e35aaf71..aa0483b1 100644 --- a/semantic_map/include/semantic_map/room_xml_parser.hpp +++ b/semantic_map/include/semantic_map/room_xml_parser.hpp @@ -55,13 +55,14 @@ bool SemanticRoomXMLParser::setRootFolderFromRoomXml(std::string room template std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoom& aRoom, std::string xmlFile, bool verbose ) { - printf("saveRoomAsXML\n"); +//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} // create folder structure QString roomLogName(aRoom.getRoomLogName().c_str()); int index = roomLogName.indexOf('_'); QString date = roomLogName.left(index); QString patrol = roomLogName.right(roomLogName.size()-index-1); +//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} QString dateFolder = QString(m_RootFolder)+date; if (!QDir(dateFolder).exists()) { @@ -74,6 +75,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoom::saveRoomAsXML(SemanticRoom::saveRoomAsXML(SemanticRoomsetDevice(&file); @@ -119,6 +123,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteCharacters(aRoom.getRoomStringId().c_str()); xmlWriter->writeEndElement(); +//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} // RoomLogStartTime xmlWriter->writeStartElement("RoomLogStartTime"); boost::posix_time::ptime roomLogStartTime = aRoom.getRoomLogStartTime(); @@ -137,14 +142,30 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteAttribute("filename",cloudFilename); xmlWriter->writeEndElement(); +//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} if (aRoom.getCompleteRoomCloudLoaded()) // only save the cloud file if it's been loaded { + //if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} QFile file(completeCloudFilename); // if (!file.exists()) { + //if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} if (aRoom.getCompleteRoomCloud()->points.size()>0) { - pcl::io::savePCDFileBinary(completeCloudFilename.toStdString(), *aRoom.getCompleteRoomCloud()); + + //if(verbose){printf("aRoom.getCompleteRoomCloud()->points.size(): %i\n",aRoom.getCompleteRoomCloud()->points.size());} + //if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} + std::string fullcloudpath = completeCloudFilename.toStdString(); + if(verbose){printf("fullcloudpath: %s\n",fullcloudpath.c_str());} + + //CloudPtr fullcld = aRoom.getCompleteRoomCloud(); + + Cloud fullcld = *(aRoom.getCompleteRoomCloud()); + + //pcl::io::savePCDFileBinary(completeCloudFilename.toStdString(), *aRoom.getCompleteRoomCloud()); + //pcl::io::savePCDFileBinary(fullcloudpath, *fullcld); + pcl::io::savePCDFileBinaryCompressed(fullcloudpath, fullcld); + //if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} if (verbose) { ROS_INFO_STREAM("Saving complete cloud file name "<::saveRoomAsXML(SemanticRoomwriteStartElement("RoomInteriorCloud"); QString interiorCloudFilenameLocal("interior_cloud.pcd"); @@ -175,7 +196,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteStartElement("RoomDeNoisedCloud"); QString denoisedCloudFilenameLocal("denoised_cloud.pcd"); @@ -197,12 +218,12 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoom::saveRoomAsXML(SemanticRoomwriteEndElement(); } } - +//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} // RoomCentroid @@ -240,7 +261,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteCharacters(centroidS); xmlWriter->writeEndElement(); - +//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} // Room Transform xmlWriter->writeStartElement("RoomTransform"); Eigen::Matrix4f transform = aRoom.getRoomTransform(); @@ -251,7 +272,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteCharacters(transformS); xmlWriter->writeEndElement(); - +//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} // RoomIntermediateClouds xmlWriter->writeStartElement("RoomIntermediateClouds"); xmlWriter->writeAttribute("pan_start",QString::number(aRoom.m_SweepParameters.m_pan_start)); @@ -292,7 +313,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteStartElement("RoomIntermediateCloudTransform"); @@ -346,7 +367,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteEndElement(); // RoomIntermediateCloudTransform - +//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} // RoomIntermediateCloudRegisteredTransform std::vector roomIntermediateCloudTransformsRegistered = aRoom.getIntermediateCloudTransformsRegistered(); // TODO: this is not the case as I might be registering only the lower sweep. @@ -369,7 +390,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteAttribute("width",QString::number(camInfo.width)); @@ -396,7 +417,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteAttribute("D",DString); // ROS_INFO_STREAM("D matrix "<::saveRoomAsXML(SemanticRoomwriteEndElement(); // RoomIntermediateClouds - +//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} // Intermediate cloud images saveIntermediateImagesToXML(aRoom,xmlWriter, roomFolder.toStdString()); diff --git a/strands_sweep_registration/include/strands_sweep_registration/Camera.h b/strands_sweep_registration/include/strands_sweep_registration/Camera.h index e8bbcde6..b7f7effe 100644 --- a/strands_sweep_registration/include/strands_sweep_registration/Camera.h +++ b/strands_sweep_registration/include/strands_sweep_registration/Camera.h @@ -20,10 +20,16 @@ class Camera { int version; PixelFunction ** pixelsFunctions; + + unsigned int coefs_degree; + unsigned int coefs_width; + unsigned int coefs_height; + double * multiplierCoeffs; Camera( float fx_, float fy_, float cx_, float cy_, int width_, int height_); virtual ~Camera(); + virtual double getMultiplier(double w, double h, double z); virtual void save(std::string path); virtual void load(std::string path); virtual void show(); diff --git a/strands_sweep_registration/include/strands_sweep_registration/Frame.h b/strands_sweep_registration/include/strands_sweep_registration/Frame.h index 68e337ff..1bff8978 100644 --- a/strands_sweep_registration/include/strands_sweep_registration/Frame.h +++ b/strands_sweep_registration/include/strands_sweep_registration/Frame.h @@ -29,6 +29,7 @@ class Frame std::vector keypoint_location; + Frame(Camera * camera_); Frame(Camera * camera_, float * rgb_data_, float * depth_data_); Frame(Camera * camera_, std::vector k, std::vector depth, cv::Mat d); // initialize from ORB features driectly ~Frame(); diff --git a/strands_sweep_registration/include/strands_sweep_registration/ProblemFrameConnection.h b/strands_sweep_registration/include/strands_sweep_registration/ProblemFrameConnection.h index 9f53d6f3..77302b1b 100644 --- a/strands_sweep_registration/include/strands_sweep_registration/ProblemFrameConnection.h +++ b/strands_sweep_registration/include/strands_sweep_registration/ProblemFrameConnection.h @@ -15,10 +15,12 @@ class ProblemFrameConnection { std::vector< Eigen::Vector3f > full_src_points; std::vector< Eigen::Vector3f > full_dst_points; + std::vector< float > full_noisemul; std::vector< Eigen::Vector3f > src_points; std::vector< Eigen::Vector3f > dst_points; + std::vector< float > noisemul; std::vector< float > possible_matches_fdistance; std::vector< float > possible_matches_edistance; @@ -29,7 +31,7 @@ class ProblemFrameConnection { std::vector dst_matches; ProblemFrameConnection(ceres::Problem & problem, Frame * src_, Frame * dst_, double * shared_params, double * src_variable_, double * dst_variable_, float weight = 1, bool show = false); - void addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions); + void addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions, double hubersize = 0.1); void addMatchesToProblem(ceres::Problem & problem, float weight = 1); void findPossibleMatches(float di = 1, float pi = 0, Eigen::Matrix4f pose = Eigen::Matrix4f::Identity()); void recalculatePoints(); diff --git a/strands_sweep_registration/include/strands_sweep_registration/RobotContainer.h b/strands_sweep_registration/include/strands_sweep_registration/RobotContainer.h index da6e8ce6..63690178 100644 --- a/strands_sweep_registration/include/strands_sweep_registration/RobotContainer.h +++ b/strands_sweep_registration/include/strands_sweep_registration/RobotContainer.h @@ -10,6 +10,8 @@ #include "ProblemFrameConnection.h" #include +#include +#include class RobotContainer { @@ -35,6 +37,12 @@ class RobotContainer std::vector sweeps; std::vector alignedSweep; + + std::vector paths; + + bool viewer_initialized; + boost::shared_ptr viewer; + RobotContainer(unsigned int gx_,unsigned int todox_,unsigned int gy_,unsigned int todoy_); ~RobotContainer(); @@ -51,6 +59,8 @@ class RobotContainer void saveAllSweeps(std::string path); + void show(); + private: void saveSweep(Sweep*, std::string path); diff --git a/strands_sweep_registration/src/Camera.cpp b/strands_sweep_registration/src/Camera.cpp index b253d042..6196eeb0 100644 --- a/strands_sweep_registration/src/Camera.cpp +++ b/strands_sweep_registration/src/Camera.cpp @@ -17,16 +17,40 @@ Camera::Camera(float fx_, float fy_, float cx_, float cy_, int width_, int heigh for(int i = 0; i < width*height; i++){ pixelsFunctions[i] = new PixelFunction(); } + + + coefs_degree = 1; + coefs_width = width/40; + coefs_height = height/40; + + + multiplierCoeffs = new double[coefs_degree*coefs_width*coefs_height]; version = 2; + + getMultiplier(55, 55, 1); }; Camera::~Camera(){ + delete[] multiplierCoeffs; + for(int i = 0; i < width*height; i++){ delete pixelsFunctions[i]; } delete[] pixelsFunctions; -}; +} + +double Camera::getMultiplier(double w, double h, double z){ + double wgrid = w/(coefs_width+1); + double hgrid = h/(coefs_height+1); + + double pw = wgrid-double(int(wgrid)); + double ph = hgrid-double(int(hgrid)); + + printf("%f %f -> %f %f -> %f %f\n",w,h,wgrid,hgrid,pw,ph); +exit(0); + return 1; +} void Camera::save(std::string path){ std::vector pixeldata_char; diff --git a/strands_sweep_registration/src/Frame.cpp b/strands_sweep_registration/src/Frame.cpp index c8364170..02a776ae 100644 --- a/strands_sweep_registration/src/Frame.cpp +++ b/strands_sweep_registration/src/Frame.cpp @@ -15,6 +15,13 @@ int frame_id = 0; +Frame::Frame(Camera * camera_){ + id = frame_id; + frame_id++; + camera = camera_; + featuretype = 0; +} + Frame::Frame(Camera * camera_, float * rgb_data_, float * depth_data_){ id = frame_id; frame_id++; @@ -133,6 +140,7 @@ void Frame::recalculateFullPoints(){ float centerY = camera->cy; float invFocalX = 1.0f/camera->fx; float invFocalY = 1.0f/camera->fy; + //keypoint_location.clear(); //We should probably clear this.... for(unsigned int i = 0; i < keypoints.size(); i++){ float w = keypoints.at(i).pt.x; float h = keypoints.at(i).pt.y; diff --git a/strands_sweep_registration/src/ProblemFrameConnection.cpp b/strands_sweep_registration/src/ProblemFrameConnection.cpp index b9194654..93ec4462 100644 --- a/strands_sweep_registration/src/ProblemFrameConnection.cpp +++ b/strands_sweep_registration/src/ProblemFrameConnection.cpp @@ -13,9 +13,11 @@ ProblemFrameConnection::ProblemFrameConnection(ceres::Problem & problem, Frame * recalculatePoints(); } -void ProblemFrameConnection::addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions){ - for(unsigned int i = 0; i < costfunctions.size() && i < 1000; i++ ){ - problem.AddResidualBlock(costfunctions.at(i), 0 , src_variable, dst_variable, params); +void ProblemFrameConnection::addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions, double hubersize){ + //printf("addMatchesToProblem: %i\n",costfunctions.size()); + for(unsigned int i = 0; i < costfunctions.size() && i < 2000; i++ ){ + //problem.AddResidualBlock(costfunctions.at(i), 0 , src_variable, dst_variable, params); + problem.AddResidualBlock(costfunctions.at(i), new ceres::HuberLoss(hubersize) , src_variable, dst_variable, params); } } @@ -33,7 +35,7 @@ void ProblemFrameConnection::addMatchesToProblem(ceres::Problem & problem, float //CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight/pow(sz*sz+dz*dz,2)); CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight); problem.AddResidualBlock(err, 0, src_variable, dst_variable, params); - problem.AddResidualBlock(err, new ceres::HuberLoss(0.1), src_variable, dst_variable, params); + //problem.AddResidualBlock(err, new ceres::HuberLoss(0.1), src_variable, dst_variable, params); } } @@ -165,6 +167,7 @@ void ProblemFrameConnection::findPossibleMatches(float di, float pi, Eigen::Matr void ProblemFrameConnection::recalculatePoints(){ src_points.clear(); dst_points.clear(); + noisemul.clear(); for(unsigned int i = 0; i < src_possible_matches_id.size(); i++){ int src_kp_id = src_possible_matches_id.at(i); @@ -184,6 +187,10 @@ void ProblemFrameConnection::recalculatePoints(){ src_points.push_back(Eigen::Vector3f(sx,sy,sz)); dst_points.push_back(Eigen::Vector3f(dx,dy,dz)); + double snoise = sz*sz; + double dnoise = dz*dz; + double noise = sqrt( snoise*snoise + dnoise*dnoise ); + noisemul.push_back( noise ); } } diff --git a/strands_sweep_registration/src/RobotContainer.cpp b/strands_sweep_registration/src/RobotContainer.cpp index 7136eb5a..04e5876a 100644 --- a/strands_sweep_registration/src/RobotContainer.cpp +++ b/strands_sweep_registration/src/RobotContainer.cpp @@ -12,532 +12,788 @@ typedef typename SimpleSummaryParser::EntityStruct Entities; void RobotContainer::saveAllSweeps(std::string savePath) { - for (auto sweep: sweeps) - { - saveSweep(sweep, savePath); - } + for (auto sweep: sweeps) + { + saveSweep(sweep, savePath); + } } void RobotContainer::saveSweep(Sweep * sweep, std::string savePath){//std::vector transforms) - /* std::string sweep_xml = sweep->xmlpath; - std::vector poses = sweep->getPoseVector(); - - std::cout<<"Sweep xml "<::loadRoomFromXML(sweep_xml, true); - room.clearIntermediateCloudRegisteredTransforms(); - room.clearIntermediateCloudCameraParametersCorrected(); - auto original_transforms = room.getIntermediateCloudTransforms(); - auto intClouds = room.getIntermediateClouds(); - auto original_params = room.getIntermediateCloudCameraParameters(); - - static tf::StampedTransform firstTransform = original_transforms[0]; - - Camera * camera = sweep->frames[0][0]->camera; - for (unsigned int i=0; i()); - tf::transformEigenToTF(eigenTr, tfTr); - tf::Transform combinedTransform = firstTransform * tfTr; - - sensor_msgs::CameraInfo camInfo; - camInfo.P = {camera->fx, 0.0, camera->cx, 0.0, 0.0, camera->fy, camera->cy, 0.0,0.0, 0.0, 1.0,0.0}; - camInfo.D = {0,0,0,0,0}; - image_geometry::PinholeCameraModel aCameraModel; - aCameraModel.fromCameraInfo(camInfo); - room.addIntermediateCloudCameraParametersCorrected(aCameraModel); - - transform.setOrigin(tfTr.getOrigin()); - transform.setBasis(tfTr.getBasis()); - room.addIntermediateRoomCloudRegisteredTransform(transform); - } - - SemanticRoomXMLParser parser(savePath); - parser.saveRoomAsXML(room); - */ + /* std::string sweep_xml = sweep->xmlpath; + std::vector poses = sweep->getPoseVector(); + + std::cout<<"Sweep xml "<::loadRoomFromXML(sweep_xml, true); + room.clearIntermediateCloudRegisteredTransforms(); + room.clearIntermediateCloudCameraParametersCorrected(); + auto original_transforms = room.getIntermediateCloudTransforms(); + auto intClouds = room.getIntermediateClouds(); + auto original_params = room.getIntermediateCloudCameraParameters(); + + static tf::StampedTransform firstTransform = original_transforms[0]; + + Camera * camera = sweep->frames[0][0]->camera; + for (unsigned int i=0; i()); + tf::transformEigenToTF(eigenTr, tfTr); + tf::Transform combinedTransform = firstTransform * tfTr; + + sensor_msgs::CameraInfo camInfo; + camInfo.P = {camera->fx, 0.0, camera->cx, 0.0, 0.0, camera->fy, camera->cy, 0.0,0.0, 0.0, 1.0,0.0}; + camInfo.D = {0,0,0,0,0}; + image_geometry::PinholeCameraModel aCameraModel; + aCameraModel.fromCameraInfo(camInfo); + room.addIntermediateCloudCameraParametersCorrected(aCameraModel); + + transform.setOrigin(tfTr.getOrigin()); + transform.setBasis(tfTr.getBasis()); + room.addIntermediateRoomCloudRegisteredTransform(transform); + } + + SemanticRoomXMLParser parser(savePath); + parser.saveRoomAsXML(room); + */ } RobotContainer::RobotContainer(unsigned int gx_,unsigned int todox_,unsigned int gy_,unsigned int todoy_){ - gx = gx_; - todox = todox_; - gy = gy_; - todoy = todoy_; - - inds = new unsigned int*[todox]; - for(unsigned int x = 0; x < todox; x++){inds[x] = new unsigned int[todoy];} - - for(unsigned int y = 0; y < gy; y++){//Camera moving forward - for (unsigned int x = 0; x < gx ; x++){inds[x][y] = 0;} - if(y % 2 == 0){ - for (unsigned int x = 0; x < gx ; x++){inds[x][y] = y*gx+x;} - }else{ - for (unsigned int x = 0; x < gx ; x++){inds[x][y] = y*gx+gx-x-1;} - } - } - - poses = new double**[todox]; - for(unsigned int x = 0; x < todox; x++){ - poses[x] = new double*[todoy]; - for(unsigned int y = 0; y < todoy; y++){ - poses[x][y] = new double[6]; - for(unsigned int k = 0; k < 6; k++){poses[x][y][k] = 0;} - } - } - - width = 0; - height = 0; - - shared_params = new double[5]; - camera = 0; - rgb = 0; - depth = 0; + gx = gx_; + todox = todox_; + gy = gy_; + todoy = todoy_; + + inds = new unsigned int*[todox]; + for(unsigned int x = 0; x < todox; x++){inds[x] = new unsigned int[todoy];} + + for(unsigned int y = 0; y < gy; y++){//Camera moving forward + for (unsigned int x = 0; x < gx ; x++){inds[x][y] = 0;} + if(y % 2 == 0){ + for (unsigned int x = 0; x < gx ; x++){inds[x][y] = y*gx+x;} + }else{ + for (unsigned int x = 0; x < gx ; x++){inds[x][y] = y*gx+gx-x-1;} + } + } + + poses = new double**[todox]; + for(unsigned int x = 0; x < todox; x++){ + poses[x] = new double*[todoy]; + for(unsigned int y = 0; y < todoy; y++){ + poses[x][y] = new double[6]; + for(unsigned int k = 0; k < 6; k++){poses[x][y][k] = 0;} + } + } + + width = 0; + height = 0; + + shared_params = new double[5]; + camera = 0; + rgb = 0; + depth = 0; + + viewer_initialized = false; } RobotContainer::~RobotContainer(){ - for(unsigned int x = 0; x < todox; x++){delete[] inds[x];} - delete[] inds; - - for(unsigned int s = 0; s < sweeps.size(); s++){delete sweeps.at(s);} - if(camera != 0){ delete camera;} - if(shared_params != 0){ delete shared_params;} - if(rgb != 0){ delete rgb;} - if(depth != 0){ delete depth;} - - for(unsigned int x = 0; x < todox; x++){ - for(unsigned int y = 0; y < todoy; y++){ - delete[] poses[x][y]; - } - delete[] poses[x]; - } - delete[] poses; + for(unsigned int x = 0; x < todox; x++){delete[] inds[x];} + delete[] inds; + + for(unsigned int s = 0; s < sweeps.size(); s++){delete sweeps.at(s);} + if(camera != 0){ delete camera;} + if(shared_params != 0){ delete shared_params;} + if(rgb != 0){ delete rgb;} + if(depth != 0){ delete depth;} + + for(unsigned int x = 0; x < todox; x++){ + for(unsigned int y = 0; y < todoy; y++){ + delete[] poses[x][y]; + } + delete[] poses[x]; + } + delete[] poses; }; void RobotContainer::initializeCamera(double fx, double fy, double cx, double cy, unsigned int w, unsigned int h) { - std::cout<<"Initializing camera with parameters "<version = 1; + camera = new Camera(fx, fy, cx, cy, width, height); + camera->version = 1; - rgb = new float[3*width*height]; - depth = new float[ width*height]; + rgb = new float[3*width*height]; + depth = new float[ width*height]; - shared_params[0] = 1.0/fx; //invfx - shared_params[1] = 1.0/fy; //invfy - shared_params[2] = cx; - shared_params[3] = cy; - shared_params[4] = 0.1; + shared_params[0] = 1.0/fx; //invfx + shared_params[1] = 1.0/fy; //invfy + shared_params[2] = cx; + shared_params[3] = cy; + shared_params[4] = 0.1; - // initialize singleton class - CameraParameters::get(fx, fy, cx, cy ,w, h); + // initialize singleton class + CameraParameters::get(fx, fy, cx, cy ,w, h); } bool RobotContainer::addToTrainingORBFeatures(std::string path) { - std::cout<<"Adding ORB features to training from sweep "< features = semantic_map_registration_features::loadRegistrationFeaturesFromSingleSweep(path, false); - if (features.size() == 0) - { - return false; - } - - std::vector sweep_frames; - sweep_frames.resize(todox*todoy); - for(unsigned int y = 0; y < todoy; y++){ - for (unsigned int x = 0; x < todox ; x++){ - int index = inds[x][y]; - sweep_frames.at(y*todox+x) = new Frame(camera,features[index].keypoints, features[index].depths, features[index].descriptors); - sweep_frames.at(y*todox+x)->framepos = inds[x][y]; - } - } - - sweeps.push_back(new Sweep(todox, todoy, sweep_frames)); - alignedSweep.push_back(false); - sweeps.back()->xmlpath = path; - - return true; + std::cout<<"Adding ORB features to training from sweep "< features = semantic_map_registration_features::loadRegistrationFeaturesFromSingleSweep(path, false); + + if (features.size() == 0) + { + return false; + } + + std::vector sweep_frames; + sweep_frames.resize(todox*todoy); + + if (features.size() == 51){ + for(unsigned int y = 0; y < todoy; y++){ + for (unsigned int x = 0; x < todox ; x++){ + int index = inds[x][y]; + + //printf("index: %i %i -> %i\n",y,x,index); + sweep_frames.at(y*todox+x) = new Frame(camera,features[index].keypoints, features[index].depths, features[index].descriptors); + sweep_frames.at(y*todox+x)->framepos = inds[x][y]; + } + } + } else if (features.size() == 17){ + for(unsigned int y = 0; y < todoy; y++){ + for (unsigned int x = 0; x < todox ; x++){ + int index = inds[x][y]; + if(y == 0){ + sweep_frames.at(y*todox+x) = new Frame(camera,features[index].keypoints, features[index].depths, features[index].descriptors); + }else{ + sweep_frames.at(y*todox+x) = new Frame(camera); + } + sweep_frames.at(y*todox+x)->framepos = inds[x][y]; + } + } + } else if (features.size() == 9){ + for(unsigned int y = 0; y < todoy; y++){ + for (unsigned int x = 0; x < todox ; x++){ + int index = inds[x][y]; + if(y == 0 && x % 2 == 0){ + sweep_frames.at(y*todox+x) = new Frame(camera,features[index].keypoints, features[index].depths, features[index].descriptors); + }else{ + sweep_frames.at(y*todox+x) = new Frame(camera); + } + sweep_frames.at(y*todox+x)->framepos = inds[x][y]; + } + } + } else if (features.size() == 6){ + for(unsigned int y = 0; y < todoy; y++){ + for (unsigned int x = 0; x < todox ; x++){ + int index = inds[x][y]; + if(y == 0 && x % 3 == 0){ + sweep_frames.at(y*todox+x) = new Frame(camera,features[index].keypoints, features[index].depths, features[index].descriptors); + }else{ + sweep_frames.at(y*todox+x) = new Frame(camera); + } + sweep_frames.at(y*todox+x)->framepos = inds[x][y]; + } + } + } + + sweeps.push_back(new Sweep(todox, todoy, sweep_frames)); + alignedSweep.push_back(false); + sweeps.back()->xmlpath = path; + + return true; } void RobotContainer::addToTraining(std::string path){ - printf("Not supported. Add to training using precomputed ORB features. \n"); - -/* - - SimpleXMLParser simple_parser; - SimpleXMLParser::RoomData roomData = simple_parser.loadRoomFromXML(path); - if(roomData.vIntermediateRoomClouds.size() < todox*todoy){return;} - - // float fx = roomData.vIntermediateRoomCloudCamParams[0].fx(); - // float fy = roomData.vIntermediateRoomCloudCamParams[0].fy(); - // float cx = roomData.vIntermediateRoomCloudCamParams[0].cx(); - // float cy = roomData.vIntermediateRoomCloudCamParams[0].cy(); - float fx = 540.0;//roomData.vIntermediateRoomCloudCamParams[i].fx(); - float fy = 540.0;//roomData.vIntermediateRoomCloudCamParams[i].fy(); - float cx = 319.5;//roomData.vIntermediateRoomCloudCamParams[i].cx(); - float cy = 239.5;//roomData.vIntermediateRoomCloudCamParams[i].cy(); - - cv::Size res = roomData.vIntermediateRoomCloudCamParams[0].fullResolution(); - height = res.height; - width = res.width; - - if(camera == 0){ - camera = new Camera(fx, fy, cx, cy, width, height); - camera->version = 1; - rgb = new float[3*width*height]; - depth = new float[ width*height]; - - shared_params[0] = 1.0/fx; //invfx - shared_params[1] = 1.0/fy; //invfy - shared_params[2] = cx; - shared_params[3] = cy; - shared_params[4] = 0.1; - } - - std::vector sweep_frames; - sweep_frames.resize(todox*todoy); - for(unsigned int y = 0; y < todoy; y++){ - for (unsigned int x = 0; x < todox ; x++){ - pcl::PointCloud::Ptr clouddata = roomData.vIntermediateRoomClouds[inds[x][y]]; - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height; h++){ - unsigned int ind = h*width+w; - unsigned int ind3 = 3*ind; - rgb[ind3+0] = clouddata->points.at(ind).r; - rgb[ind3+1] = clouddata->points.at(ind).g; - rgb[ind3+2] = clouddata->points.at(ind).b; - depth[ind] = clouddata->points.at(ind).z; - } - } - sweep_frames.at(y*todox+x) = new Frame(camera,rgb,depth); - sweep_frames.at(y*todox+x)->framepos = inds[x][y]; - } - } - - sweeps.push_back(new Sweep(todox, todoy, sweep_frames)); - alignedSweep.push_back(false); - sweeps.back()->idtag = roomData.roomWaypointId; - sweeps.back()->xmlpath = path; - */ + printf("Not supported. Add to training using precomputed ORB features. \n"); + + /* + + SimpleXMLParser simple_parser; + SimpleXMLParser::RoomData roomData = simple_parser.loadRoomFromXML(path); + if(roomData.vIntermediateRoomClouds.size() < todox*todoy){return;} + + // float fx = roomData.vIntermediateRoomCloudCamParams[0].fx(); + // float fy = roomData.vIntermediateRoomCloudCamParams[0].fy(); + // float cx = roomData.vIntermediateRoomCloudCamParams[0].cx(); + // float cy = roomData.vIntermediateRoomCloudCamParams[0].cy(); + float fx = 540.0;//roomData.vIntermediateRoomCloudCamParams[i].fx(); + float fy = 540.0;//roomData.vIntermediateRoomCloudCamParams[i].fy(); + float cx = 319.5;//roomData.vIntermediateRoomCloudCamParams[i].cx(); + float cy = 239.5;//roomData.vIntermediateRoomCloudCamParams[i].cy(); + + cv::Size res = roomData.vIntermediateRoomCloudCamParams[0].fullResolution(); + height = res.height; + width = res.width; + + if(camera == 0){ + camera = new Camera(fx, fy, cx, cy, width, height); + camera->version = 1; + rgb = new float[3*width*height]; + depth = new float[ width*height]; + + shared_params[0] = 1.0/fx; //invfx + shared_params[1] = 1.0/fy; //invfy + shared_params[2] = cx; + shared_params[3] = cy; + shared_params[4] = 0.1; + } + + std::vector sweep_frames; + sweep_frames.resize(todox*todoy); + for(unsigned int y = 0; y < todoy; y++){ + for (unsigned int x = 0; x < todox ; x++){ + pcl::PointCloud::Ptr clouddata = roomData.vIntermediateRoomClouds[inds[x][y]]; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height; h++){ + unsigned int ind = h*width+w; + unsigned int ind3 = 3*ind; + rgb[ind3+0] = clouddata->points.at(ind).r; + rgb[ind3+1] = clouddata->points.at(ind).g; + rgb[ind3+2] = clouddata->points.at(ind).b; + depth[ind] = clouddata->points.at(ind).z; + } + } + sweep_frames.at(y*todox+x) = new Frame(camera,rgb,depth); + sweep_frames.at(y*todox+x)->framepos = inds[x][y]; + } + } + + sweeps.push_back(new Sweep(todox, todoy, sweep_frames)); + alignedSweep.push_back(false); + sweeps.back()->idtag = roomData.roomWaypointId; + sweeps.back()->xmlpath = path; + */ } std::vector< CostFunction * > getMatchesRansac(std::vector< ProblemFrameConnection * > & pc_vec, float weight = 1, float threshold = 0.015, int ransac_iter = 200000, int nr_points = 3){ - std::vector owner; - std::vector match_id; - std::vector< Eigen::Vector3f > src_points; - std::vector< Eigen::Vector3f > dst_points; - - for(unsigned int i = 0; i < pc_vec.size(); i++){ - for(unsigned int j = 0; j < pc_vec.at(i)->src_points.size(); j++){ - owner.push_back(i); - match_id.push_back(j); - src_points.push_back(pc_vec.at(i)->src_points.at(j)); - dst_points.push_back(pc_vec.at(i)->dst_points.at(j)); - } - } - - int nr_kp = src_points.size(); - std::vector< CostFunction * > errors; - if(nr_kp == 0){return errors;} - int nr_consistent = 0; - - - Eigen::Matrix4f retpose; - std::vector bestmatching; - for(int it = 0; it < ransac_iter; it++){ - //Sample points - std::vector< int > indexes; - std::vector< Eigen::Vector3f > src_samples; - std::vector< Eigen::Vector3f > dst_samples; - - for(int j = 0; j < nr_points; j++){ - int ind; - bool failed = true; - while(failed){ - ind = rand()%nr_kp; - failed = false; - for(int k = 0; k < j; k++){ - if(ind == indexes.at(k)){failed = true;} - } - } - indexes.push_back(ind); - src_samples.push_back(src_points.at(ind)); - dst_samples.push_back(dst_points.at(ind)); - } - - //Check consistency - bool consistent = true; - - for(int j = 0; j < nr_points; j++){ - for(int k = j+1; k < nr_points; k++){ - float src_distance = (src_samples.at(j)-src_samples.at(k)).norm(); - float dst_distance = (dst_samples.at(j)-dst_samples.at(k)).norm(); - if(fabs(src_distance-dst_distance) > 0.02){consistent = false; break;break;} - } - } - - //Check inliers - nr_consistent += consistent; - if(consistent){ - pcl::TransformationFromCorrespondences tfc; - for(int i = 0; i < nr_points; i++){tfc.add(dst_samples.at(i),src_samples.at(i),1);} - Eigen::Affine3f ret = tfc.getTransformation(); - - Eigen::Matrix4f pose = ret.matrix().inverse(); - - std::vector matching; - for(int j = 0; j < nr_kp; j++){ - Eigen::Vector3f src_data = src_points.at(j); - Eigen::Vector3f dst_data = dst_points.at(j); - - Eigen::Vector4f sd = pose*Eigen::Vector4f(src_data(0),src_data(1),src_data(2),1); - Eigen::Vector4f dd = Eigen::Vector4f(dst_data(0),dst_data(1),dst_data(2),1); - float t = threshold;//0.1*0.005*(dst_data(2)*dst_data(2)+src_data(2)*src_data(2)); - if((sd-dd).norm() < t){matching.push_back(j);} - } - - //save if best - if(matching.size() > bestmatching.size()){ - bestmatching = matching; - retpose = pose; - //printf("%i -> %i\n",it,matching.size()); - } - if(nr_consistent == 30000){printf("break at %i\n",it);break;} - } - } - - printf("nr matches best: %i / %i consistent: %i / %i\n",int(bestmatching.size()),nr_kp,nr_consistent,ransac_iter); - - //getMatchesICP(pc_vec,retpose); - - for(int iter = 0; iter < 200; iter++){ - bestmatching.clear(); - pcl::TransformationFromCorrespondences tfc; - int matching = 0; - for(int j = 0; j < nr_kp; j++){ - Eigen::Vector3f src_data = src_points.at(j); - Eigen::Vector3f dst_data = dst_points.at(j); - - Eigen::Vector4f sd = retpose*Eigen::Vector4f(src_data(0),src_data(1),src_data(2),1); - Eigen::Vector4f dd = Eigen::Vector4f(dst_data(0),dst_data(1),dst_data(2),1); - - if((sd-dd).norm() < threshold){ - bestmatching.push_back(j); - tfc.add(dst_data.head<3>(),src_data.head<3>(),1); - matching++; - } - } - if(iter % 50 == 0){printf("iteration: %i matches: %i\n",iter,matching);} - - Eigen::Affine3f ret = tfc.getTransformation(); - retpose = ret.matrix().inverse(); - } - printf("nr matches best: %i / %i \n",int(bestmatching.size()),nr_kp); - //exit(0); - if(bestmatching.size() < 15){return errors;} - for(unsigned int i = 0; i < pc_vec.size(); i++){ - pc_vec.at(i)->src_matches.clear(); - pc_vec.at(i)->dst_matches.clear(); - } - - for(unsigned int i = 0; i < bestmatching.size() && i < 1000; i++){ - ProblemFrameConnection * pc = pc_vec.at(owner.at(bestmatching.at(i))); - int id = match_id.at(bestmatching.at(i)); - //printf("id: %i %i %i\n ",id,pc->src_matches.size(),pc->dst_matches.size()); - int src_kp_id = pc->src_possible_matches_id.at(id); - int dst_kp_id = pc->dst_possible_matches_id.at(id); - pc->src_matches.push_back(src_kp_id); - pc->dst_matches.push_back(dst_kp_id); - cv::KeyPoint src_kp = pc->src->keypoints.at(src_kp_id); - cv::KeyPoint dst_kp = pc->dst->keypoints.at(dst_kp_id); - double sz = pc->src->keypoint_depth.at(src_kp_id); - double dz = pc->dst->keypoint_depth.at(dst_kp_id); - CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight); - errors.push_back(err); - } - - return errors; + printf("weight: %f\n",weight); + std::vector owner; + std::vector match_id; + std::vector< Eigen::Vector3f > src_points; + std::vector< Eigen::Vector3f > dst_points; + std::vector< double > noiseFactor; + + for(unsigned int i = 0; i < pc_vec.size(); i++){ + for(unsigned int j = 0; j < pc_vec.at(i)->src_points.size(); j++){ + owner.push_back(i); + match_id.push_back(j); + src_points.push_back(pc_vec.at(i)->src_points.at(j)); + dst_points.push_back(pc_vec.at(i)->dst_points.at(j)); + noiseFactor.push_back(pc_vec.at(i)->noisemul.at(j)); + } + } + + int nr_kp = src_points.size(); + std::vector< CostFunction * > errors; + if(nr_kp == 0){return errors;} + int nr_consistent = 0; + + + Eigen::Matrix4f retpose; + std::vector bestmatching; + for(int it = 0; it < ransac_iter; it++){ + //Sample points + std::vector< int > indexes; + std::vector< Eigen::Vector3f > src_samples; + std::vector< Eigen::Vector3f > dst_samples; + std::vector< double> noise_samples; + + for(int j = 0; j < nr_points; j++){ + int ind; + bool failed = true; + while(failed){ + ind = rand()%nr_kp; + failed = false; + for(int k = 0; k < j; k++){ + if(ind == indexes.at(k)){failed = true;} + } + } + indexes.push_back(ind); + src_samples.push_back(src_points.at(ind)); + dst_samples.push_back(dst_points.at(ind)); + noise_samples.push_back(noiseFactor.at(ind)); + } + + //Check consistency + bool consistent = true; + + for(int j = 0; j < nr_points; j++){ + for(int k = j+1; k < nr_points; k++){ + float src_distance = (src_samples.at(j)-src_samples.at(k)).norm(); + float dst_distance = (dst_samples.at(j)-dst_samples.at(k)).norm(); + double noise = noise_samples.at(k); + double t = threshold*noise; + + if(fabs(src_distance-dst_distance) > 4.0*t){consistent = false; break;break;} + } + } + + //Check inliers + nr_consistent += consistent; + if(consistent){ + pcl::TransformationFromCorrespondences tfc; + for(int i = 0; i < nr_points; i++){tfc.add(dst_samples.at(i),src_samples.at(i),1);} + Eigen::Affine3f ret = tfc.getTransformation(); + + Eigen::Matrix4f pose = ret.matrix().inverse(); + + std::vector matching; + for(int j = 0; j < nr_kp; j++){ + Eigen::Vector3f src_data = src_points.at(j); + Eigen::Vector3f dst_data = dst_points.at(j); + + Eigen::Vector4f sd = pose*Eigen::Vector4f(src_data(0),src_data(1),src_data(2),1); + Eigen::Vector4f dd = Eigen::Vector4f(dst_data(0),dst_data(1),dst_data(2),1); + double noise = noiseFactor[j]; + float t = threshold*noise;//0.1*0.005*(dst_data(2)*dst_data(2)+src_data(2)*src_data(2)); + if((sd-dd).norm() < t){matching.push_back(j);} + } + + //save if best + if(matching.size() > bestmatching.size()){ + bestmatching = matching; + retpose = pose; + //printf("%i -> %i\n",it,matching.size()); + } + if(nr_consistent == 2500){printf("break at %i\n",it);break;} + } + } + + printf("nr matches best: %i / %i consistent: %i / %i\n",int(bestmatching.size()),nr_kp,nr_consistent,ransac_iter); + + //getMatchesICP(pc_vec,retpose); + + for(int iter = 0; iter < 200; iter++){ + bestmatching.clear(); + pcl::TransformationFromCorrespondences tfc; + int matching = 0; + for(int j = 0; j < nr_kp; j++){ + Eigen::Vector3f src_data = src_points.at(j); + Eigen::Vector3f dst_data = dst_points.at(j); + + Eigen::Vector4f sd = retpose*Eigen::Vector4f(src_data(0),src_data(1),src_data(2),1); + Eigen::Vector4f dd = Eigen::Vector4f(dst_data(0),dst_data(1),dst_data(2),1); + double noise = noiseFactor[j]; + + if((sd-dd).norm() < noise*threshold){ + bestmatching.push_back(j); + tfc.add(dst_data.head<3>(),src_data.head<3>(),1.0/(noise*noise)); + matching++; + } + } + if(iter % 50 == 0){printf("iteration: %i matches: %i\n",iter,matching);} + + Eigen::Affine3f ret = tfc.getTransformation(); + retpose = ret.matrix().inverse(); + } + printf("nr matches best: %i / %i \n",int(bestmatching.size()),nr_kp); + //exit(0); + if(bestmatching.size() < 15){return errors;} + for(unsigned int i = 0; i < pc_vec.size(); i++){ + pc_vec.at(i)->src_matches.clear(); + pc_vec.at(i)->dst_matches.clear(); + } + + + //for(unsigned int i = 0; i < bestmatching.size() && i < 1000; i++){ + for(unsigned int i = 0; i < bestmatching.size(); i++){ + ProblemFrameConnection * pc = pc_vec.at(owner.at(bestmatching.at(i))); + int id = match_id.at(bestmatching.at(i)); + //printf("id: %i %i %i\n ",id,pc->src_matches.size(),pc->dst_matches.size()); + int src_kp_id = pc->src_possible_matches_id.at(id); + int dst_kp_id = pc->dst_possible_matches_id.at(id); + pc->src_matches.push_back(src_kp_id); + pc->dst_matches.push_back(dst_kp_id); + cv::KeyPoint src_kp = pc->src->keypoints.at(src_kp_id); + cv::KeyPoint dst_kp = pc->dst->keypoints.at(dst_kp_id); + double sz = pc->src->keypoint_depth.at(src_kp_id); + double dz = pc->dst->keypoint_depth.at(dst_kp_id); + + double snoise = sz*sz; + double dnoise = dz*dz; + double noise = sqrt( snoise*snoise + dnoise*dnoise ); + + if(bestmatching.size() > 5000 && (sz > 2.5 || dz > 2.5)){continue;} + + CostFunction* err = new pair3DError(src_kp.pt.x,src_kp.pt.y,sz,dst_kp.pt.x,dst_kp.pt.y,dz,weight/noise); + errors.push_back(err); + } + + return errors; } template Eigen::Matrix getMat(const T* const camera, int mode = 0){ - Eigen::Matrix ret = Eigen::Matrix::Identity(); - if(mode == 0){//yaw pitch roll tx ty tz - Eigen::AngleAxis yawAngle(camera[0], Eigen::Matrix::UnitY()); - Eigen::AngleAxis pitchAngle(camera[1], Eigen::Matrix::UnitX()); - Eigen::AngleAxis rollAngle(camera[2], Eigen::Matrix::UnitZ()); - Eigen::Quaternion q = rollAngle * yawAngle * pitchAngle; - Eigen::Matrix rotationMatrix = q.matrix(); - ret.block(0,0,3,3) = rotationMatrix; - ret(0,3) = camera[3]; - ret(1,3) = camera[4]; - ret(2,3) = camera[5]; - } - return ret; + Eigen::Matrix ret = Eigen::Matrix::Identity(); + if(mode == 0){//yaw pitch roll tx ty tz + Eigen::AngleAxis yawAngle(camera[0], Eigen::Matrix::UnitY()); + Eigen::AngleAxis pitchAngle(camera[1], Eigen::Matrix::UnitX()); + Eigen::AngleAxis rollAngle(camera[2], Eigen::Matrix::UnitZ()); + Eigen::Quaternion q = rollAngle * yawAngle * pitchAngle; + Eigen::Matrix rotationMatrix = q.matrix(); + ret.block(0,0,3,3) = rotationMatrix; + ret(0,3) = camera[3]; + ret(1,3) = camera[4]; + ret(2,3) = camera[5]; + } + return ret; +} + +void RobotContainer::show(){ + if(!viewer_initialized){ + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); + viewer->setBackgroundColor (1.0, 1.0, 1.0); + viewer->addCoordinateSystem (1.0); + viewer->initCameraParameters (); + viewer_initialized = true; + } + + for(unsigned int pa = 0; pa < paths.size(); pa++){ + std::string path = paths[pa]; + printf("path: %s\n",path.c_str()); + + SimpleXMLParser simple_parser; + SimpleXMLParser::RoomData roomData = simple_parser.loadRoomFromXML(path); + + //pcl::PointCloud::Ptr clouddata = roomData.vIntermediateRoomClouds[inds[x][y]]; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + + camera->fx = 1.0/shared_params[0]; camera->fy = 1.0/shared_params[1]; camera->cx = shared_params[2]; camera->cy = shared_params[3]; + camera->print(); for(unsigned int s = 0; s < sweeps.size(); s++){ + for(unsigned int x = 0; x < todox; x++){ + for(unsigned int y = 0; y < todoy; y++){ + sweeps.at(s)->poses[x][y] = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast(); + } + } + } + + std::vector registeredPoses = sweeps.at(0)->getPoseVector(); // all the sweeps have the same poses. Return the first one + + for(unsigned int i = 0; i < roomData.vIntermediateRoomClouds.size(); i++){ + if(true || (i == 0) || (i == roomData.vIntermediateRoomClouds.size()-1)){ + const float cx = camera->cx; + const float cy = camera->cy; + const float ifx = 1.0/camera->fx; + const float ify = 1.0/camera->fy; + + Eigen::Matrix4f p = registeredPoses[i]; + + float m00 = p(0,0); float m01 = p(0,1); float m02 = p(0,2); float m03 = p(0,3); + float m10 = p(1,0); float m11 = p(1,1); float m12 = p(1,2); float m13 = p(1,3); + float m20 = p(2,0); float m21 = p(2,1); float m22 = p(2,2); float m23 = p(2,3); + + pcl::PointCloud::Ptr clouddata = roomData.vIntermediateRoomClouds[i]; + + int r = rand()%256; + int g = rand()%256; + int b = rand()%256; + +// if(i == 0){r = 0; g = 255; b = 0;} +// if(i == 16){r = 255; g = 0; b = 0;} + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + pcl::PointXYZRGB & po = clouddata->points.at(ind); + double z = po.z; + double x = (w - cx) * z * ifx; + double y = (h - cy) * z * ify; + po.x = m00*x + m01*y + m02*z + m03; + po.y = m10*x + m11*y + m12*z + m13; + po.z = m20*x + m21*y + m22*z + m23; + po.r = r; + po.g = g; + po.b = b; + } + } + *cloud += *clouddata; + } + } + viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); + viewer->spin(); + viewer->removeAllPointClouds(); + } +// +// viewer->spin(); +// + +// cv::Size res = roomData.vIntermediateRoomCloudCamParams[0].fullResolution(); +// height = res.height; +// width = res.width; + +// if(camera == 0){ +// camera = new Camera(fx, fy, cx, cy, width, height); +// camera->version = 1; +// rgb = new float[3*width*height]; +// depth = new float[ width*height]; + +// shared_params[0] = 1.0/fx; //invfx +// shared_params[1] = 1.0/fy; //invfy +// shared_params[2] = cx; +// shared_params[3] = cy; +// shared_params[4] = 0.1; +// } + + + + +// sweep_frames.at(y*todox+x) = new Frame(camera,rgb,depth); +// sweep_frames.at(y*todox+x)->framepos = inds[x][y]; +// } +// } + +// sweeps.push_back(new Sweep(todox, todoy, sweep_frames)); +// alignedSweep.push_back(false); +// sweeps.back()->idtag = roomData.roomWaypointId; +// sweeps.back()->xmlpath = path; + + + //SemanticRoom aRoom = SemanticRoomXMLParser::loadRoomFromXML(path,true); + //exit(0); + } std::vector RobotContainer::runInitialTraining(){ - ceres::Problem problem; - Solver::Options options; - options.max_num_iterations = 1500; - options.minimizer_progress_to_stdout = true; - options.num_linear_solver_threads = 11; - options.num_threads = 11; - Solver::Summary summary; - - // double *** poses = new double**[todox]; - // for(unsigned int x = 0; x < todox; x++){ - // poses[x] = new double*[todoy]; - // for(unsigned int y = 0; y < todoy; y++){ - // poses[x][y] = new double[6]; - // for(unsigned int k = 0; k < 6; k++){poses[x][y][k] = 0;} - // } - // } - - - //1st forward X loop - std::vector< std::vector< ProblemFrameConnection * > > x1_vec; - x1_vec.resize(todoy); - for(unsigned int s = 0; s < sweeps.size(); s++){ - printf("1st forward X loop: %i\n",s); - Sweep * sweep = sweeps.at(s); - for(unsigned int x = 0; x < todox-1; x++){ - for(unsigned int y = 0; y < todoy; y++){ - ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x+1][y], shared_params, poses[x][y], poses[x+1][y]); - x1_vec.at(y).push_back(pc); - } - } - } - - for(unsigned int y = 0; y < todoy; y++){ - std::vector< CostFunction * > matches = getMatchesRansac(x1_vec.at(y)); - for(unsigned int i = 0; i < x1_vec.at(y).size(); i++){ - x1_vec.at(y).at(i)->addMatchesToProblem(problem, matches); - } - } - - Solve(options, &problem, &summary); - - pair3DError::optimizeCameraParams = true; - Solve(options, &problem, &summary); - - //1st forward Y loop - std::vector< std::vector< ProblemFrameConnection * > > y1_vec; - y1_vec.resize(todox); - for(unsigned int s = 0; s < sweeps.size(); s++){ - printf("1st forward Y loop: %i\n",s); - Sweep * sweep = sweeps.at(s); - for(unsigned int x = 0; x < todox; x++){ - for(unsigned int y = 0; y < todoy-1; y++){ - ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x][y+1], shared_params, poses[x][y], poses[x][y+1]); - y1_vec.at(x).push_back(pc); - } - } - } - - for(unsigned int x = 0; x < todox; x++){ - std::vector< CostFunction * > matches = getMatchesRansac(y1_vec.at(x)); - for(unsigned int i = 0; i < y1_vec.at(x).size(); i++){ - y1_vec.at(x).at(i)->addMatchesToProblem(problem, matches); - } - } - - Solve(options, &problem, &summary); - //std::cout << summary.FullReport() << "\n"; - - //2nd forward X loop - std::vector< std::vector< ProblemFrameConnection * > > x2_vec; - x2_vec.resize(todoy); - for(unsigned int s = 0; s < sweeps.size(); s++){ - printf("2t forward X loop: %i\n",s); - Sweep * sweep = sweeps.at(s); - for(unsigned int x = 0; x < todox-2; x++){ - for(unsigned int y = 0; y < todoy; y++){ - ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x+2][y], shared_params, poses[x][y], poses[x+2][y]); - x2_vec.at(y).push_back(pc); - } - } - } - - for(unsigned int y = 0; y < todoy; y++){ - std::vector< CostFunction * > matches = getMatchesRansac(x2_vec.at(y)); - for(unsigned int i = 0; i < x2_vec.at(y).size(); i++){ - x2_vec.at(y).at(i)->addMatchesToProblem(problem, matches); - } - } - - Solve(options, &problem, &summary); - //std::cout << summary.FullReport() << "\n"; - - // camera->fx = 1.0/shared_params[0]; camera->fy = 1.0/shared_params[1]; camera->cx = shared_params[2]; camera->cy = shared_params[3]; - // camera->print(); - // for(unsigned int s = 0; s < sweeps.size(); s++){ - // for(unsigned int x = 0; x < todox; x++){ - // for(unsigned int y = 0; y < todoy; y++){ - // sweeps.at(s)->poses[x][y] = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast(); - // } - // } - // } - - //Loop closure - std::vector< std::vector< ProblemFrameConnection * > > loop_vec; - loop_vec.resize(todoy); - for(unsigned int s = 0; s < sweeps.size(); s++){ - printf("Loop closure: %i\n",s); - Sweep * sweep = sweeps.at(s); - for(unsigned int y = 0; y < todoy; y++){ - ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[0][y],sweep->frames[todox-1][y], shared_params, poses[0][y], poses[todox-1][y],1,false); - loop_vec.at(y).push_back(pc); - } - } - - for(unsigned int y = 0; y < todoy; y++){ - std::vector< CostFunction * > matches = getMatchesRansac(loop_vec.at(y),x1_vec.at(y).size()); - for(unsigned int i = 0; i < loop_vec.at(y).size(); i++){ - loop_vec.at(y).at(i)->addMatchesToProblem(problem, matches); - } - } - Solve(options, &problem, &summary); - std::cout << summary.FullReport() << "\n"; - - //Optimize camera parameter - //optimizeCameraParams = true; - //Solve(options, &problem, &summary); - //std::cout << summary.FullReport() << "\n"; - - camera->fx = 1.0/shared_params[0]; camera->fy = 1.0/shared_params[1]; camera->cx = shared_params[2]; camera->cy = shared_params[3]; - camera->print(); for(unsigned int s = 0; s < sweeps.size(); s++){ - for(unsigned int x = 0; x < todox; x++){ - for(unsigned int y = 0; y < todoy; y++){ - sweeps.at(s)->poses[x][y] = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast(); - } - } - } - - std::vector registeredPoses = sweeps.at(0)->getPoseVector(); // all the sweeps have the same poses. Return the first one - return registeredPoses; + + double start_fx = camera->fx; + double start_fy = camera->fy; + double start_cx = camera->cx; + double start_cy = camera->cy; + + + +////for(double hs = 0.5; hs >= 0.0001; hs *= 0.5){ +////for(double current_fy = start_fy-100.0; current_fy <= start_fy+101.0; current_fy += 10.0){ +//{ +//// shared_params[0] = 1.0/start_fx; //invfx +//// shared_params[1] = 1.0/current_fy; //invfy +//// shared_params[2] = start_cx; +//// shared_params[3] = start_cy; +//// shared_params[4] = 0.1; + +// ceres::Problem problem; +// Solver::Options options; +// options.max_num_iterations = 1500; +// options.minimizer_progress_to_stdout = true; +// options.num_linear_solver_threads = 11; +// options.num_threads = 11; +// Solver::Summary summary; + +// //Loop closure +// std::vector< std::vector< ProblemFrameConnection * > > loop_vec; +// loop_vec.resize(todoy); +// for(unsigned int s = 0; s < sweeps.size(); s++){ +// printf("Loop closure: %i\n",s); +// Sweep * sweep = sweeps.at(s); +// for(unsigned int y = 0; y < todoy; y++){ +// ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[0][y],sweep->frames[todox-1][y], shared_params, poses[0][y], poses[todox-1][y],1,false); +// loop_vec.at(y).push_back(pc); +// } +// } + +// for(unsigned int y = 0; y < todoy; y++){ +// std::vector< CostFunction * > matches = getMatchesRansac(loop_vec.at(y)); +// for(unsigned int i = 0; i < loop_vec.at(y).size(); i++){ +// loop_vec.at(y).at(i)->addMatchesToProblem(problem, matches,1.0); +// } +// } + + +// Solve(options, &problem, &summary); +// std::cout << summary.FullReport() << "\n"; +// show(); + + + +//// for(double current_fy = start_fy-300.0; current_fy <= start_fy+501.0; current_fy += 50.0){ +//// shared_params[0] = 1.0/start_fx; //invfx +//// shared_params[1] = 1.0/current_fy; //invfy +//// shared_params[2] = start_cx; +//// shared_params[3] = start_cy; +//// shared_params[4] = 0.1; +//// show(); +//// }; +//} + +ceres::Problem problem; +Solver::Options options; +options.max_num_iterations = 1500; +options.minimizer_progress_to_stdout = true; +options.num_linear_solver_threads = 11; +options.num_threads = 11; +Solver::Summary summary; + + + // double *** poses = new double**[todox]; + // for(unsigned int x = 0; x < todox; x++){ + // poses[x] = new double*[todoy]; + // for(unsigned int y = 0; y < todoy; y++){ + // poses[x][y] = new double[6]; + // for(unsigned int k = 0; k < 6; k++){poses[x][y][k] = 0;} + // } + // } + + + //1st forward X loop + std::vector< std::vector< ProblemFrameConnection * > > x1_vec; + x1_vec.resize(todoy); + for(unsigned int s = 0; s < sweeps.size(); s++){ + printf("1st forward X loop: %i\n",s); + Sweep * sweep = sweeps.at(s); + for(unsigned int x = 0; x < todox-1; x++){ + for(unsigned int y = 0; y < todoy; y++){ + ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x+1][y], shared_params, poses[x][y], poses[x+1][y]); + x1_vec.at(y).push_back(pc); + } + } + } + + + for(unsigned int y = 0; y < todoy; y++){ + std::vector< CostFunction * > matches = getMatchesRansac(x1_vec.at(y)); + for(unsigned int i = 0; i < x1_vec.at(y).size(); i++){ + if(i % 100 == 0){printf("%i\n",i);} + x1_vec.at(y).at(i)->addMatchesToProblem(problem, matches); + if(y == 0 && i == 300){Solve(options, &problem, &summary);} + } + } + + Solve(options, &problem, &summary); +//show(); + + //1st forward Y loop + std::vector< std::vector< ProblemFrameConnection * > > y1_vec; + y1_vec.resize(todox); + for(unsigned int s = 0; s < sweeps.size(); s++){ + printf("1st forward Y loop: %i\n",s); + Sweep * sweep = sweeps.at(s); + for(unsigned int x = 0; x < todox; x++){ + for(unsigned int y = 0; y < todoy-1; y++){ + ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x][y+1], shared_params, poses[x][y], poses[x][y+1]); + y1_vec.at(x).push_back(pc); + } + } + } + + for(unsigned int x = 0; x < todox; x++){ + std::vector< CostFunction * > matches = getMatchesRansac(y1_vec.at(x)); + for(unsigned int i = 0; i < y1_vec.at(x).size(); i++){ + y1_vec.at(x).at(i)->addMatchesToProblem(problem, matches); + } + } + + + Solve(options, &problem, &summary); +//show(); + + //std::cout << summary.FullReport() << "\n"; + + //2nd forward X loop + std::vector< std::vector< ProblemFrameConnection * > > x2_vec; + x2_vec.resize(todoy); + for(unsigned int s = 0; s < sweeps.size(); s++){ + printf("2t forward X loop: %i\n",s); + Sweep * sweep = sweeps.at(s); + for(unsigned int x = 0; x < todox-2; x++){ + for(unsigned int y = 0; y < todoy; y++){ + ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[x][y],sweep->frames[x+2][y], shared_params, poses[x][y], poses[x+2][y]); + x2_vec.at(y).push_back(pc); + } + } + } + + + for(unsigned int y = 0; y < todoy; y++){ + std::vector< CostFunction * > matches = getMatchesRansac(x2_vec.at(y)); + for(unsigned int i = 0; i < x2_vec.at(y).size(); i++){ + x2_vec.at(y).at(i)->addMatchesToProblem(problem, matches); + } + } + + Solve(options, &problem, &summary); +//show(); + //std::cout << summary.FullReport() << "\n"; + + // camera->fx = 1.0/shared_params[0]; camera->fy = 1.0/shared_params[1]; camera->cx = shared_params[2]; camera->cy = shared_params[3]; + // camera->print(); + // for(unsigned int s = 0; s < sweeps.size(); s++){ + // for(unsigned int x = 0; x < todox; x++){ + // for(unsigned int y = 0; y < todoy; y++){ + // sweeps.at(s)->poses[x][y] = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast(); + // } + // } + // } + + //Loop closure + std::vector< std::vector< ProblemFrameConnection * > > loop_vec; + loop_vec.resize(todoy); + for(unsigned int s = 0; s < sweeps.size(); s++){ + printf("Loop closure: %i\n",s); + Sweep * sweep = sweeps.at(s); + for(unsigned int y = 0; y < todoy; y++){ + ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[0][y],sweep->frames[todox-1][y], shared_params, poses[0][y], poses[todox-1][y],1,false); + loop_vec.at(y).push_back(pc); + } + } + + for(unsigned int y = 0; y < todoy; y++){ + std::vector< CostFunction * > matches = getMatchesRansac(loop_vec.at(y),x1_vec.at(y).size()); + for(unsigned int i = 0; i < loop_vec.at(y).size(); i++){ + loop_vec.at(y).at(i)->addMatchesToProblem(problem, matches); + } + } + + + Solve(options, &problem, &summary); + std::cout << summary.FullReport() << "\n"; +show(); + +// pair3DError::optimizeCameraParams = true; +// Solve(options, &problem, &summary); +//show(); + //Optimize camera parameter + //optimizeCameraParams = true; + //Solve(options, &problem, &summary); + //std::cout << summary.FullReport() << "\n"; + + camera->fx = 1.0/shared_params[0]; camera->fy = 1.0/shared_params[1]; camera->cx = shared_params[2]; camera->cy = shared_params[3]; + camera->print(); for(unsigned int s = 0; s < sweeps.size(); s++){ + for(unsigned int x = 0; x < todox; x++){ + for(unsigned int y = 0; y < todoy; y++){ + sweeps.at(s)->poses[x][y] = (getMat(poses[0][0]).inverse()*getMat(poses[x][y])).cast(); + } + } + } + + std::vector registeredPoses = sweeps.at(0)->getPoseVector(); // all the sweeps have the same poses. Return the first one + + +//show(); + + return registeredPoses; } void RobotContainer::refineTraining(){} std::vector RobotContainer::train(){ - return runInitialTraining(); - // refineTraining(); + return runInitialTraining(); + // refineTraining(); } bool RobotContainer::isCalibrated(){return true;} @@ -546,45 +802,45 @@ bool RobotContainer::isCalibrated(){return true;} std::vector RobotContainer::alignAndStoreSweeps(){ - // aligns sweeps to the first one - - using namespace Eigen; - - std::vector toRet; - - if (sweeps.size() == 0) - { - std::cout<<"No sweeps to register."<(); - sweeps.at(s)->poses[x][y] = mat; - } - } - } - - Sweep * sweep = sweeps.at(0); - for(unsigned int i = 1; i < sweeps.size(); i++){ - Sweep * s = sweeps.at(i); - Eigen::Matrix4f m = sweep->align(s); - toRet.push_back(m); - } - return toRet; + // aligns sweeps to the first one + + using namespace Eigen; + + std::vector toRet; + + if (sweeps.size() == 0) + { + std::cout<<"No sweeps to register."<(); + sweeps.at(s)->poses[x][y] = mat; + } + } + } + + Sweep * sweep = sweeps.at(0); + for(unsigned int i = 1; i < sweeps.size(); i++){ + Sweep * s = sweeps.at(i); + Eigen::Matrix4f m = sweep->align(s); + toRet.push_back(m); + } + return toRet; } From 5e216b393dafd1a1101baf183744ee2c8158c026 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 16 Nov 2016 14:29:02 +0100 Subject: [PATCH 211/255] working on sweep registration --- .../strands_sweep_registration/Camera.h | 4 ++- strands_sweep_registration/src/Camera.cpp | 33 ++++++++++++++----- .../src/ProblemFrameConnection.cpp | 2 +- .../src/RobotContainer.cpp | 25 +++++++------- 4 files changed, 42 insertions(+), 22 deletions(-) diff --git a/strands_sweep_registration/include/strands_sweep_registration/Camera.h b/strands_sweep_registration/include/strands_sweep_registration/Camera.h index b7f7effe..702561a4 100644 --- a/strands_sweep_registration/include/strands_sweep_registration/Camera.h +++ b/strands_sweep_registration/include/strands_sweep_registration/Camera.h @@ -28,7 +28,9 @@ class Camera { Camera( float fx_, float fy_, float cx_, float cy_, int width_, int height_); virtual ~Camera(); - + + virtual double * getCoeffs(unsigned int w, unsigned int h); + virtual double getGridMultiplier(unsigned int w, unsigned int h, double z); virtual double getMultiplier(double w, double h, double z); virtual void save(std::string path); virtual void load(std::string path); diff --git a/strands_sweep_registration/src/Camera.cpp b/strands_sweep_registration/src/Camera.cpp index 6196eeb0..f6c3a533 100644 --- a/strands_sweep_registration/src/Camera.cpp +++ b/strands_sweep_registration/src/Camera.cpp @@ -18,21 +18,29 @@ Camera::Camera(float fx_, float fy_, float cx_, float cy_, int width_, int heigh pixelsFunctions[i] = new PixelFunction(); } + version = 2; - coefs_degree = 1; - coefs_width = width/40; - coefs_height = height/40; +// coefs_degree = 1; +// coefs_width = width/40; +// coefs_height = height/40; - multiplierCoeffs = new double[coefs_degree*coefs_width*coefs_height]; - - version = 2; +// multiplierCoeffs = new double[coefs_degree*coefs_width*coefs_height]; +// for(int w = 0; w < width; w++){ +// for(int h = 0; h < height; h++){ +// double * coeffs = getCoeffs(w,h); +// coeffs[0] = 1.0; +// for(unsigned int i = 1; i < coefs_degree; i++){ +// coeffs[i] = 0.0; +// } +// } +// } - getMultiplier(55, 55, 1); + //getMultiplier(55, 55, 1); }; Camera::~Camera(){ - delete[] multiplierCoeffs; + //delete[] multiplierCoeffs; for(int i = 0; i < width*height; i++){ delete pixelsFunctions[i]; @@ -40,6 +48,15 @@ Camera::~Camera(){ delete[] pixelsFunctions; } +double * Camera::getCoeffs(unsigned int w, unsigned int h){ + return multiplierCoeffs + coefs_degree*(coefs_width*h + w); +} + +double Camera::getGridMultiplier(unsigned int w, unsigned int h, double z){ + double * coeffs = getCoeffs(w,h); + return 0; +} + double Camera::getMultiplier(double w, double h, double z){ double wgrid = w/(coefs_width+1); double hgrid = h/(coefs_height+1); diff --git a/strands_sweep_registration/src/ProblemFrameConnection.cpp b/strands_sweep_registration/src/ProblemFrameConnection.cpp index 93ec4462..7be63e01 100644 --- a/strands_sweep_registration/src/ProblemFrameConnection.cpp +++ b/strands_sweep_registration/src/ProblemFrameConnection.cpp @@ -15,7 +15,7 @@ ProblemFrameConnection::ProblemFrameConnection(ceres::Problem & problem, Frame * void ProblemFrameConnection::addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions, double hubersize){ //printf("addMatchesToProblem: %i\n",costfunctions.size()); - for(unsigned int i = 0; i < costfunctions.size() && i < 2000; i++ ){ + for(unsigned int i = 0; i < costfunctions.size() && i < 1000; i++ ){ //problem.AddResidualBlock(costfunctions.at(i), 0 , src_variable, dst_variable, params); problem.AddResidualBlock(costfunctions.at(i), new ceres::HuberLoss(hubersize) , src_variable, dst_variable, params); } diff --git a/strands_sweep_registration/src/RobotContainer.cpp b/strands_sweep_registration/src/RobotContainer.cpp index 04e5876a..f89a192e 100644 --- a/strands_sweep_registration/src/RobotContainer.cpp +++ b/strands_sweep_registration/src/RobotContainer.cpp @@ -462,7 +462,7 @@ void RobotContainer::show(){ viewer_initialized = true; } - for(unsigned int pa = 0; pa < paths.size(); pa++){ + for(unsigned int pa = 0; pa < paths.size(); pa += 60){ std::string path = paths[pa]; printf("path: %s\n",path.c_str()); @@ -648,12 +648,12 @@ Solver::Summary summary; // } // } - + printf("1st forward X loop\n"); //1st forward X loop std::vector< std::vector< ProblemFrameConnection * > > x1_vec; x1_vec.resize(todoy); for(unsigned int s = 0; s < sweeps.size(); s++){ - printf("1st forward X loop: %i\n",s); + Sweep * sweep = sweeps.at(s); for(unsigned int x = 0; x < todox-1; x++){ for(unsigned int y = 0; y < todoy; y++){ @@ -667,20 +667,20 @@ Solver::Summary summary; for(unsigned int y = 0; y < todoy; y++){ std::vector< CostFunction * > matches = getMatchesRansac(x1_vec.at(y)); for(unsigned int i = 0; i < x1_vec.at(y).size(); i++){ - if(i % 100 == 0){printf("%i\n",i);} + //if(i % 100 == 0){printf("%i\n",i);} x1_vec.at(y).at(i)->addMatchesToProblem(problem, matches); - if(y == 0 && i == 300){Solve(options, &problem, &summary);} + //if(y == 0 && i == 300){Solve(options, &problem, &summary);} } } - Solve(options, &problem, &summary); + //Solve(options, &problem, &summary); //show(); - + printf("1st forward Y loop:\n"); //1st forward Y loop std::vector< std::vector< ProblemFrameConnection * > > y1_vec; y1_vec.resize(todox); for(unsigned int s = 0; s < sweeps.size(); s++){ - printf("1st forward Y loop: %i\n",s); + Sweep * sweep = sweeps.at(s); for(unsigned int x = 0; x < todox; x++){ for(unsigned int y = 0; y < todoy-1; y++){ @@ -702,12 +702,12 @@ Solver::Summary summary; //show(); //std::cout << summary.FullReport() << "\n"; + printf("2t forward X loop\n"); //2nd forward X loop std::vector< std::vector< ProblemFrameConnection * > > x2_vec; x2_vec.resize(todoy); for(unsigned int s = 0; s < sweeps.size(); s++){ - printf("2t forward X loop: %i\n",s); Sweep * sweep = sweeps.at(s); for(unsigned int x = 0; x < todox-2; x++){ for(unsigned int y = 0; y < todoy; y++){ @@ -725,7 +725,7 @@ Solver::Summary summary; } } - Solve(options, &problem, &summary); + //Solve(options, &problem, &summary); //show(); //std::cout << summary.FullReport() << "\n"; @@ -740,10 +740,11 @@ Solver::Summary summary; // } //Loop closure + printf("Loop closure\n"); std::vector< std::vector< ProblemFrameConnection * > > loop_vec; loop_vec.resize(todoy); for(unsigned int s = 0; s < sweeps.size(); s++){ - printf("Loop closure: %i\n",s); + Sweep * sweep = sweeps.at(s); for(unsigned int y = 0; y < todoy; y++){ ProblemFrameConnection * pc = new ProblemFrameConnection(problem, sweep->frames[0][y],sweep->frames[todox-1][y], shared_params, poses[0][y], poses[todox-1][y],1,false); @@ -761,7 +762,7 @@ Solver::Summary summary; Solve(options, &problem, &summary); std::cout << summary.FullReport() << "\n"; -show(); +//show(); // pair3DError::optimizeCameraParams = true; // Solve(options, &problem, &summary); From ae82e02f75b6ab6954b694fcce25271ab4e3bd1a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 18 Nov 2016 16:28:47 +0100 Subject: [PATCH 212/255] test on tsc data --- .../quasimodo_brain/src/AnnotationTool.cpp | 5 +- quasimodo/quasimodo_brain/src/Util/Util.cpp | 16 +- .../metaroom_additional_view_processing.cpp | 247 +++++++----- quasimodo/quasimodo_brain/src/modelserver.cpp | 56 ++- .../quasimodo_launch/launch/quasimodo.launch | 15 +- .../quasimodo_models/include/core/Util.h | 1 + .../include/registration/Registration.h | 2 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 4 +- .../quasimodo_models/src/model/Model.cpp | 3 +- .../src/registration/MassRegistrationPPR2.cpp | 19 +- .../src/registration/Registration.cpp | 10 +- .../src/registration/RegistrationRandom.cpp | 369 ++++++++++++++---- .../registration/RegistrationRefinement.cpp | 256 +++++++----- 13 files changed, 703 insertions(+), 300 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp index 5476db8a..5df94ba7 100644 --- a/quasimodo/quasimodo_brain/src/AnnotationTool.cpp +++ b/quasimodo/quasimodo_brain/src/AnnotationTool.cpp @@ -604,8 +604,9 @@ bool annotate(std::string path){ cv::namedWindow( "Annotation tool", cv::WINDOW_AUTOSIZE ); cv::imshow( "Annotation tool",img ); char c = cv::waitKey(0); - - if(c == -29){ +printf("%i\n",int(c)); + if(c == 27){break;} + else if(c == -29){ state = 4; }else if(state == 0){ if((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '9') || c == ' '){classname += c;} diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index c9c1d915..e0f98774 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -260,7 +260,7 @@ Eigen::Matrix4d getMat(tf::StampedTransform tf){ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath){ int slash_pos = sweep_xml.find_last_of("/"); std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; - printf("load_metaroom_model(%s)\n",sweep_folder.c_str()); +// printf("load_metaroom_model(%s)\n",sweep_folder.c_str()); SimpleXMLParser parser; std::vector dummy; @@ -280,12 +280,12 @@ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath) std::vector current_room_frames; - printf("roomData.vIntermediateRoomClouds.size() = %i\n",roomData.vIntermediateRoomClouds.size()); - printf("roomData.vIntermediateRoomCloudCamParamsCorrected.size() = %i\n",roomData.vIntermediateRoomCloudCamParamsCorrected.size()); - printf("roomData.vIntermediateRoomCloudTransformsRegistered.size() = %i\n",roomData.vIntermediateRoomCloudTransformsRegistered.size()); +// printf("roomData.vIntermediateRoomClouds.size() = %i\n",roomData.vIntermediateRoomClouds.size()); +// printf("roomData.vIntermediateRoomCloudCamParamsCorrected.size() = %i\n",roomData.vIntermediateRoomCloudCamParamsCorrected.size()); +// printf("roomData.vIntermediateRoomCloudTransformsRegistered.size() = %i\n",roomData.vIntermediateRoomCloudTransformsRegistered.size()); for (size_t i=0; i bgs, std::vector< reglib::Model * > viewer->setBackgroundColor(1.0,1.0,1.0); } - reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.05); + reglib::MassRegistrationPPR2 * massregmod = new reglib::MassRegistrationPPR2(0.15); if(savePath.size() != 0){ massregmod->savePath = savePath+"/segment_"+std::to_string(models.front()->id); } massregmod->timeout = 1200; massregmod->viewer = viewer; massregmod->visualizationLvl = debugg > 1; - massregmod->maskstep = 10;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massregmod->nomaskstep = 10;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massregmod->maskstep = 11;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massregmod->nomaskstep = 11;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massregmod->nomask = true; massregmod->stopval = 0.0001; diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index b222923d..c7f7f284 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -641,7 +641,7 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); } - +//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( sweep, reg); mu->occlusion_penalty = 15; @@ -649,13 +649,13 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri mu->viewer = viewer; sweep->recomputeModelPoints(); - +//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); reglib::Model * fullmodel = new reglib::Model(); fullmodel->savePath = savePath+"/"; fullmodel->frames = sweep->frames; fullmodel->relativeposes = sweep->relativeposes; fullmodel->modelmasks = sweep->modelmasks; - +//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); if(frames.size() > 0){ reglib::RegistrationRefinement * refinement = new reglib::RegistrationRefinement(); @@ -665,12 +665,14 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri refinement->target_points = 4000; ////double register_setup_start = getTime(); + //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); reglib::CloudData * cd1 = sweep->getCD(sweep->points.size()); refinement->setDst(cd1); fullmodel->points = frames.front()->getSuperPoints(Eigen::Matrix4d::Identity(),1,false); reglib::CloudData * cd2 = fullmodel->getCD(fullmodel->points.size()); refinement->setSrc(cd2); + //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); //////printf("register_setup_start: %5.5f\n",getTime()-register_setup_start); ////double register_compute_start = getTime(); Eigen::Matrix4d guess = both_unrefined[1]*both_unrefined[0].inverse(); @@ -691,6 +693,7 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri if(savePath.size() != 0){ bgmassreg->savePath = savePath+"/processAV_"+std::to_string(fullmodel->id); } + //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); bgmassreg->timeout = 300; bgmassreg->viewer = viewer; @@ -703,6 +706,8 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri bgmassreg->stopval = 0.0005; bgmassreg->addModel(sweep); bgmassreg->setData(frames,masks); +// exit(0); + //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); @@ -849,9 +854,12 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: readViewXML(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes,compute_edges,saveVisuals_sp); fullmodel->recomputeModelPoints(); }else{ + //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); fullmodel = processAV(path,compute_edges,saveVisuals_sp); + //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); } + //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); return fullmodel; } @@ -875,7 +883,14 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr store_old_xml = objectFiles.size() == 0; SimpleXMLParser parser; - SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path,std::vector{"RoomIntermediateCloud","IntermediatePosition"}); + + SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path,std::vector()); +// if(current_roomData.roomWaypointId.compare("ReceptionDesk") != 0){return 0;} + + current_roomData = parser.loadRoomFromXML(path,std::vector{"RoomIntermediateCloud","IntermediatePosition"}); + if(current_roomData.vIntermediateRoomClouds.size() != 17){return 0;} + + reglib::Model * fullmodel = getAVMetaroom(path,true,saveVisuals); @@ -954,7 +969,7 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr auto sweep = SimpleXMLParser::loadRoomFromXML(path, std::vector{},false); printf("models.front()->frames.size() = %i\n",models.front()->frames.size()); - quasimodo_brain::segment(bgs,models,internal,external,dynamic,visualization_lvl > 0,saveVisuals); + quasimodo_brain::segment(bgs,models,internal,external,dynamic,visualization_lvl,saveVisuals); remove_old_seg(sweep_folder); @@ -1321,6 +1336,7 @@ void chatterCallback(const std_msgs::String::ConstPtr& msg){ void trainMetaroom(std::string path){ printf("processing: %s\n",path.c_str()); + if(posepath.compare("")==0){ printf("posepath not set, set before training\n"); return ; @@ -1333,10 +1349,10 @@ void trainMetaroom(std::string path){ //Not needed if metaroom well calibrated reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.01); - bgmassreg->timeout = 600; + bgmassreg->timeout = 60*10; bgmassreg->viewer = viewer; bgmassreg->use_surface = true; - bgmassreg->use_depthedge = true; + bgmassreg->use_depthedge = false;//true; bgmassreg->visualizationLvl = visualization_lvl;//0; bgmassreg->maskstep = 5; bgmassreg->nomaskstep = 5; @@ -1344,7 +1360,7 @@ void trainMetaroom(std::string path){ bgmassreg->stopval = 0.0005; bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); - +exit(0); savePoses(overall_folder+"/"+posepath,bgmfr.poses,17); fullmodel->fullDelete(); delete fullmodel; @@ -1663,6 +1679,8 @@ std::vector getRoomSuperPoints(std::string path, std::string std::ifstream file (sweep_folder+"/superpoints.bin", ios::in|ios::binary|ios::ate); if (file.is_open()) { size = file.tellg(); + if(size == 0){return spvec;} + memblock = new char [size]; file.seekg (0, ios::beg); file.read (memblock, size); @@ -1696,7 +1714,6 @@ std::vector getRoomSuperPoints(std::string path, std::string delete[] memblock; }else{ std::cout << "Unable to open file superpoint file, go process that data" << std::endl; - processSweepForDatabase(sweep_folder+"/room.xml",savePath); } return spvec; @@ -1774,47 +1791,66 @@ void saveSuperPoints(std::string path, std::vector & spvec, void processSweepForDatabase(std::string path, std::string savePath){ path = replaceAll(path, "//", "/"); - printf("processSweepForDatabase(%s)\n",path.c_str()); if ( ! boost::filesystem::exists( path ) ){return;} int slash_pos = path.find_last_of("/"); std::string sweep_folder = path.substr(0, slash_pos) + "/"; - printf("sweep_folder: %s\n",sweep_folder.c_str()); + SimpleXMLParser parser; SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml",std::vector(),false,false); std::string current_waypointid = roomData.roomWaypointId; + + if(current_waypointid.compare("ReceptionDesk") != 0){return;} + //if(current_waypointid.compare("WayPoint5") != 0){return;} + + + printf("------------------------START--------------------------\n"); + printf("processSweepForDatabase(%s)\n",path.c_str()); + //printf("sweep_folder: %s\n",sweep_folder.c_str()); + printf("current_waypointid: %s\n",current_waypointid.c_str()); + + //Send the previous room to the modelserver... int firstind = -1; int prevind = -1; - std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder,false); for (unsigned int i = 0; i < sweep_xmls.size(); i++){ sweep_xmls[i] = replaceAll(sweep_xmls[i], "//", "/"); SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); + //printf("sweep: %s\n",sweep_xmls[i].c_str()); + if(sweep_xmls[i].compare(path) == 0){break;} std::string other_waypointid = other_roomData.roomWaypointId; - if(firstind == -1 && other_waypointid.compare(current_waypointid) == 0){firstind = i;} - if(other_waypointid.compare(current_waypointid) == 0){prevind = i;} + std::string other_path = sweep_xmls[i]; + int slash_pos = other_path.find_last_of("/"); + std::string other_sweep_folder = other_path.substr(0, slash_pos) + "/"; + std::ifstream file (other_sweep_folder+"/superpoints.bin", ios::in|ios::binary|ios::ate); + if (file.is_open()) { + file.close(); + + //Check if it has the superpoint file + if(other_waypointid.compare(current_waypointid) == 0){ + if(firstind == -1){firstind = i;} + prevind = i; + } + } } - printf("%i %i\n",prevind,firstind); - + reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); + Eigen::Matrix4d change = Eigen::Matrix4d::Identity(); if(firstind != -1){ - processSweepForDatabase(sweep_xmls[firstind],savePath); - printf("first: %s\n",sweep_xmls[firstind].c_str()); std::vector backgroundsp = getRoomSuperPoints(sweep_xmls[firstind],savePath); if(prevind != -1 && prevind != firstind){ - printf("prev: %s\n",sweep_xmls[prevind].c_str()); std::vector prev_vec = getRoomSuperPoints(sweep_xmls[prevind],savePath); backgroundsp.insert(backgroundsp.end(), prev_vec.begin(), prev_vec.end()); } - Eigen::Matrix4d currentpose = Eigen::Matrix4d::Identity(); roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); if(roomData.vIntermediateRoomCloudTransforms.size() != 0){ @@ -1828,10 +1864,28 @@ void processSweepForDatabase(std::string path, std::string savePath){ firstpose = quasimodo_brain::getMat(first_roomData.vIntermediateRoomCloudTransforms[0]); } + std::vector poses; + poses.push_back(Eigen::Matrix4d::Identity()); + poses.push_back(sweep->frames.front()->pose);//Eigen::Matrix4d::Identity()); + reglib::Model * bgdata = new reglib::Model(); + bgdata->points = backgroundsp; - exit(0); - + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.1); + bgmassreg->timeout = 600; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = true; + bgmassreg->visualizationLvl = visualization_lvl;//0; + bgmassreg->maskstep = 5; + bgmassreg->nomaskstep = 5; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->addModel(bgdata); + bgmassreg->addModel(sweep); + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(poses); + change = bgmfr.poses.back() * poses.back().inverse(); + pose = bgmfr.poses.back(); }else{//this is the first room, set to whatever odometry is printf("this is the first room, set to whatever odometry is!\n"); roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); @@ -1840,71 +1894,83 @@ void processSweepForDatabase(std::string path, std::string savePath){ } } - std::cout << pose << std::endl << std::endl; - - reglib::Model * sweep = quasimodo_brain::load_metaroom_model(path,savePath); - saveSuperPoints(sweep_folder+"/superpoints.bin",sweep->points,pose); -// std::vector testvec = getRoomSuperPoints(sweep_folder+"/superpoints.bin",savePath); - - - exit(0); - roomData = parser.loadRoomFromXML(sweep_folder+"/room.xml"); - - - // ros::ServiceClient client = n->serviceClient("/soma_llsd/insert_scene"); - // ROS_INFO("Waiting for /soma_llsd/insert_scene service..."); - // if (!client.waitForExistence(ros::Duration(1.0))) { - // ROS_INFO("Failed to get /soma_llsd/insert_scene service!"); - // return; - // } - // ROS_INFO("Got /soma_llsd/insert_scene service"); - - // segment.id = std::to_string(model.model_id); - - // size_t counter = 0; - // for (const quasimodo_msgs::rgbd_frame& frame : model.frames) { - - // if (!client.call(scene)) { - // ROS_ERROR("Failed to call service /soma_llsd/insert_scene"); - // return; - // } - - - - - - - //Get first sweep and previous sweep, merge those - //Get first sweep and previous sweep, merge those - - - - // for(unsigned int i = 0; i < sweep->frames.size(); i++){ - // reglib::RGBDFrame * frame = sweep->frames[i]; - // geometry_msgs::Pose pose; - // tf::poseEigenToMsg (Eigen::Affine3d(frame->pose), pose); + saveSuperPoints(sweep_folder+"/superpoints.bin",sweep->points,pose,1); + for(unsigned int i = 0; i < sweep->frames.size(); i++){ + sweep->frames[i]->pose *= change; + } - // cv_bridge::CvImage rgbBridgeImage; - // rgbBridgeImage.image = frame->rgb; - // rgbBridgeImage.encoding = "bgr8"; - // cv_bridge::CvImage depthBridgeImage; - // depthBridgeImage.image = frame->depth; - // depthBridgeImage.encoding = "mono16"; +// string meta_data +// uint32 timestamp +// tf/tfMessage transform +// --- +// bool result +// soma_llsd_msgs/Scene response + + ros::ServiceClient client = np->serviceClient("/soma_llsd/insert_scene"); + ROS_INFO("Waiting for /soma_llsd/insert_scene service..."); + if (!client.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/insert_scene service!"); + //return; + }else{ + ROS_INFO("Got /soma_llsd/insert_scene service"); + } - // soma_llsd::InsertScene scene; - // scene.request.rgb_img = *(rgbBridgeImage.toImageMsg()); - // scene.request.depth_img = *(depthBridgeImage.toImageMsg()); - // scene.request.camera_info.K[0] = frame->camera->fx; - // scene.request.camera_info.K[4] = frame->camera->fy; - // scene.request.camera_info.K[2] = frame->camera->cx; - // scene.request.camera_info.K[5] = frame->camera->cy; - // //scene.request.robot_pose = frame.pose; // not good actually but whatevs - - // sensor_msgs::PointCloud2 input; - // pcl::toROSMsg (*(roomData.vIntermediateRoomClouds[i]),input);//, *transformed_cloud); - // input.header.frame_id = "/map"; - // scene.request.cloud = input; - // } + soma_llsd_msgs::Segment sweepsegment; + sweepsegment.id = "sweep"+roomData.roomRunNumber; + + for(unsigned int i = 0; i < sweep->frames.size(); i++){ + reglib::RGBDFrame * frame = sweep->frames[i]; + geometry_msgs::Pose pose; + tf::poseEigenToMsg (Eigen::Affine3d(frame->pose), pose); + + geometry_msgs::Pose segpose; + tf::poseEigenToMsg (Eigen::Affine3d(sweep->relativeposes[i]), segpose); + + std::cout << sweep->relativeposes[i] << std::endl; + + cv_bridge::CvImage rgbBridgeImage; + rgbBridgeImage.image = frame->rgb; + rgbBridgeImage.encoding = "bgr8"; + + cv_bridge::CvImage depthBridgeImage; + depthBridgeImage.image = frame->depth; + depthBridgeImage.encoding = "mono16"; + + sensor_msgs::PointCloud2 input; + pcl::toROSMsg (*(roomData.vIntermediateRoomClouds[i]),input);//, *transformed_cloud); + input.header.frame_id = "/map"; + + soma_llsd::InsertScene scene; + scene.request.rgb_img = *(rgbBridgeImage.toImageMsg()); + scene.request.depth_img = *(depthBridgeImage.toImageMsg()); + scene.request.camera_info.K[0] = frame->camera->fx; + scene.request.camera_info.K[4] = frame->camera->fy; + scene.request.camera_info.K[2] = frame->camera->cx; + scene.request.camera_info.K[5] = frame->camera->cy; + scene.request.robot_pose = pose; + scene.request.cloud = input; + scene.request.waypoint = current_waypointid; + scene.request.episode_id = roomData.roomRunNumber; + +// if (!client.call(scene)) { +// ROS_ERROR("Failed to call service /soma_llsd/insert_scene"); +// return; +// } + + + cv_bridge::CvImage maskBridgeImage; + maskBridgeImage.image = sweep->modelmasks[i]->getMask(); + maskBridgeImage.encoding = "mono8"; + + soma_llsd_msgs::Observation obs; + obs.scene_id = scene.response.response.id; + //obs.camera_cloud = model.clouds[counter]; //Dont add clouds... + obs.image_mask = *(maskBridgeImage.toImageMsg()); + obs.pose = segpose; + obs.id = "frame"+std::to_string(frame->id); +// obs.timestamp = ros::Time::now().nsec; + sweepsegment.observations.push_back(obs); + } // int startsize = msg.local_poses.size(); @@ -1949,7 +2015,7 @@ void processSweepForDatabase(std::string path, std::string savePath){ sweep->fullDelete(); delete sweep; - exit(0); + // exit(0); // int additional_nrviews = 0; // QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); // for (auto objectFile : objectFiles){ @@ -1996,6 +2062,7 @@ void processSweepForDatabase(std::string path, std::string savePath){ // quasimodo_msgs::model model; // soma_llsd_msgs::Segment segment; //model_to_soma_segment(ros::NodeHandle& n, const quasimodo_msgs::model& model, soma_llsd_msgs::Segment& segment); + printf("------------------------STOP--------------------------\n"); } bool testPath(std::string path){ @@ -2003,7 +2070,7 @@ bool testPath(std::string path){ vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(path); for (auto sweep_xml : sweep_xmls) { - printf("sweep_xml: %s\n",sweep_xml.c_str()); + //printf("sweep_xml: %s\n",sweep_xml.c_str()); //processSweep(sweep_xml,""); processSweepForDatabase(sweep_xml,""); } @@ -2082,7 +2149,7 @@ int main(int argc, char** argv){ else if(std::string(argv[i]).compare("-v_init") == 0){ visualization_lvl_regini = 1;inputstate = 17;} else if(std::string(argv[i]).compare("-v_reg") == 0){ visualization_lvl_regref = 1;inputstate = 18;} else if(std::string(argv[i]).compare("-saveVisuals") == 0){ inputstate = 19;} - else if(std::string(argv[i]).compare("-testpaths") == 0){ inputstate = 20;} + else if(std::string(argv[i]).compare("-testpaths") == 0 || std::string(argv[i]).compare("-testpath") == 0 || std::string(argv[i]).compare("-testPaths") == 0 || std::string(argv[i]).compare("-testPath") == 0){ inputstate = 20;} else if(inputstate == 0){ segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); }else if(inputstate == 1){ @@ -2126,10 +2193,10 @@ int main(int argc, char** argv){ saveVisuals = std::string(argv[i]); }else if(inputstate == 20){ testpaths.push_back(std::string(argv[i])); - printf("testpaths: %i\n",testpaths.size()); } } + printf("done reading commands.\n"); if(visualization_lvl != 0 || visualization_lvl_regref != 0 || visualization_lvl_regini != 0){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("3D Viewer")); @@ -2177,6 +2244,8 @@ int main(int argc, char** argv){ for(unsigned int i = 0; i < testpaths.size(); i++){ testPath(testpaths[i]); } + + for(unsigned int i = 0; i < raresfiles.size(); i++){segmentRaresFiles( raresfiles[i], raresfiles_resegment[i]);} for(unsigned int i = 0; i < trainMetarooms.size(); i++){ trainMetaroom( trainMetarooms[i]);} for(unsigned int i = 0; i < processMetarooms.size(); i++){ diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 7eca4aa1..5c0cd196 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -56,6 +56,9 @@ using namespace quasimodo_brain; bool visualization = false; bool show_db = false;//Full database show +bool save_db = false;//Full database save +int save_db_counter = 0; + int show_init_lvl = 0;//init show int show_refine_lvl = 0;//refine show int show_reg_lvl = 0;//registration show @@ -109,7 +112,7 @@ int sweepid_counter = 0; int current_model_update = 0; -bool myfunction (reglib::Model * i,reglib::Model * j) { return i->frames.size() > j->frames.size(); } +bool myfunction (reglib::Model * i,reglib::Model * j) { return (i->frames.size() + i->submodels.size()) > (j->frames.size() + j->submodels.size()); } quasimodo_msgs::retrieval_result sresult; bool new_search_result = false; @@ -192,9 +195,10 @@ void retrievalCallback(const quasimodo_msgs::retrieval_query_result & qr){ } void showModels(std::vector mods){ - viewer->removeAllPointClouds(); + //viewer->removeAllPointClouds(); float maxx = 0; + pcl::PointCloud::Ptr conccloud (new pcl::PointCloud); for(unsigned int i = 0; i < mods.size(); i++){ pcl::PointCloud::Ptr cloud = mods[i]->getPCLcloud(1, false); float meanx = 0; @@ -220,17 +224,36 @@ void showModels(std::vector mods){ for(unsigned int j = 0; j < cloud->points.size(); j++){minx = std::min(cloud->points[j].x , minx);} for(unsigned int j = 0; j < cloud->points.size(); j++){cloud->points[j].x += maxx-minx + 0.15;} for(unsigned int j = 0; j < cloud->points.size(); j++){maxx = std::max(cloud->points[j].x,maxx);} + +// char buf [1024]; +// sprintf(buf,"cloud%i",i); +// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); + + *conccloud += *cloud; + } + + if( save_db ){ + printf("save_db\n"); char buf [1024]; - sprintf(buf,"cloud%i",i); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); + sprintf(buf,"quasimodoDB_%i.pcd",save_db_counter); + pcl::io::savePCDFileBinaryCompressed(buf, *conccloud); + save_db_counter++; } - viewer->spin(); + + if(show_db && visualization){ + viewer->removeAllPointClouds(); + viewer->addPointCloud (conccloud, pcl::visualization::PointCloudColorHandlerRGBField(conccloud), "conccloud"); + viewer->spin(); + } + +// printf("show_sorted()\n"); +// exit(0); } int savecounter = 0; void show_sorted(){ - if(!show_db){return;} - if(!visualization){return;} + if(!show_db && !save_db ){return;} + //if(!visualization){return;} std::vector results; for(unsigned int i = 0; i < modeldatabase->models.size(); i++){results.push_back(modeldatabase->models[i]);} std::sort (results.begin(), results.end(), myfunction); @@ -397,8 +420,8 @@ bool indexFrame(quasimodo_msgs::index_frame::Request & req, quasimodo_msgs::ind bool addToDB(ModelDatabase * database, reglib::Model * model, bool add);//, bool deleteIfFail = false); bool addIfPossible(ModelDatabase * database, reglib::Model * model, reglib::Model * model2){ -// std::map new_models; -// std::map updated_models; + + printf("addIfPossible\n"); reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model2, reg); @@ -465,7 +488,7 @@ bool addToDB(ModelDatabase * database, reglib::Model * model, bool add){// = tru model->last_changed = ++current_model_update; } - std::vector res = modeldatabase->search(model,3); + std::vector res = modeldatabase->search(model,1); if(show_search){showModels(res);} for(unsigned int i = 0; i < res.size(); i++){ @@ -659,27 +682,23 @@ bool runSearch(ModelDatabase * database, reglib::Model * model, int number_of_se std::set searchset; void addNewModel(reglib::Model * model){ -printf("%s::%i\n",__FILE__,__LINE__); reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model, reg); mu->occlusion_penalty = occlusion_penalty; mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; -printf("%s::%i\n",__FILE__,__LINE__); mu->show_init_lvl = show_init_lvl;//init show mu->show_refine_lvl = show_refine_lvl;//refine show mu->show_scoring = show_scoring;//fuse scoring show reg->visualizationLvl = show_reg_lvl; -printf("%s::%i\n",__FILE__,__LINE__); mu->makeInitialSetup(); delete mu; delete reg; -printf("%s::%i\n",__FILE__,__LINE__); reglib::Model * newmodelHolder = new reglib::Model(); newmodelHolder->submodels.push_back(model); @@ -690,12 +709,11 @@ printf("%s::%i\n",__FILE__,__LINE__); }else{ newmodelHolder->recomputeModelPoints(); } -printf("%s::%i\n",__FILE__,__LINE__); + modeldatabase->add(newmodelHolder); addToDB(modeldatabase, newmodelHolder,false); show_sorted(); -printf("%s::%i\n",__FILE__,__LINE__); bool do_next = true; while(do_next && run_search){ printf("running search loop\n"); @@ -720,12 +738,9 @@ printf("%s::%i\n",__FILE__,__LINE__); for(unsigned int i = 0; i < modeldatabase->models.size(); i++){publish_history(modeldatabase->models[i]->getHistory());} publishDatabasePCD(); dumpDatabase(savepath); - -printf("%s::%i\n",__FILE__,__LINE__); } void modelCallback(const quasimodo_msgs::model & m){ - printf("%s::%i\n",__FILE__,__LINE__); quasimodo_msgs::model mod = m; addNewModel(quasimodo_brain::getModelFromMSG(mod,false)); } @@ -742,8 +757,6 @@ bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_ uint64 frame_id = req.frame_id; uint64 isnewmodel = req.isnewmodel; - //printf("%i and %i\n",int(frame_id),int(isnewmodel)); - printf("modelFromFrame %i and %i\n",int(frame_id),int(isnewmodel)); cv_bridge::CvImagePtr mask_ptr; @@ -1326,6 +1339,7 @@ int main(int argc, char **argv){ else if(std::string(argv[i]).compare("-v_register") == 0 || std::string(argv[i]).compare("-v_reg") == 0){ printf("visualization registration turned on\n"); visualization = true; inputstate = 10;} else if(std::string(argv[i]).compare("-v_scoring") == 0 || std::string(argv[i]).compare("-v_score") == 0 || std::string(argv[i]).compare("-v_sco") == 0){ printf("visualization scoring turned on\n"); visualization = true; show_scoring = true;} else if(std::string(argv[i]).compare("-v_db") == 0){ printf("visualization db turned on\n"); visualization = true; show_db = true;} + else if(std::string(argv[i]).compare("-s_db") == 0){ printf("save db turned on\n"); save_db = true;} else if(std::string(argv[i]).compare("-intopic") == 0){ printf("intopic input state\n"); inputstate = 11;} else if(std::string(argv[i]).compare("-mdb") == 0){ printf("intopic input state\n"); inputstate = 12;} else if(std::string(argv[i]).compare("-show_search") == 0){ printf("show_search\n"); show_search = true;} diff --git a/quasimodo/quasimodo_launch/launch/quasimodo.launch b/quasimodo/quasimodo_launch/launch/quasimodo.launch index 7abcc149..ee54d00f 100644 --- a/quasimodo/quasimodo_launch/launch/quasimodo.launch +++ b/quasimodo/quasimodo_launch/launch/quasimodo.launch @@ -1,6 +1,7 @@ - + + @@ -10,6 +11,7 @@ + + + + + + + + diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index a34733ce..1de3fe18 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -75,6 +75,7 @@ std::vector partition_graph(std::vector< std::vector< float > > & scores); inline double getNoise(double depth){return depth*depth;} inline double getInformation(double depth){ + if(depth == 0 || depth > 4){return 0.00000000001;} double n = getNoise(depth); return 1.0/(n*n); } diff --git a/quasimodo/quasimodo_models/include/registration/Registration.h b/quasimodo/quasimodo_models/include/registration/Registration.h index 290286b5..29aa199c 100644 --- a/quasimodo/quasimodo_models/include/registration/Registration.h +++ b/quasimodo/quasimodo_models/include/registration/Registration.h @@ -82,7 +82,7 @@ namespace reglib void setVisualizationLvl(unsigned int lvl); virtual FusionResults getTransform(Eigen::MatrixXd guess); - virtual void show(Eigen::MatrixXd X, Eigen::MatrixXd Y); + virtual void show(Eigen::MatrixXd X, Eigen::MatrixXd Y, bool stop = true); virtual void show(Eigen::MatrixXd X, Eigen::MatrixXd Y, Eigen::VectorXd W); virtual void show(Eigen::MatrixXd X, Eigen::MatrixXd Xn, Eigen::MatrixXd Y, Eigen::MatrixXd Yn); diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index b0c970e1..2d95e84d 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -329,7 +329,7 @@ RGBDFrame * RGBDFrame::clone(){ } RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals, std::string savePath, bool compute_imgedges){ - printf("savepath: %s\n",savePath.c_str()); + //printf("savepath: %s\n",savePath.c_str()); bool verbose = false; if(verbose) printf("------------------------------\n"); @@ -839,7 +839,7 @@ if(compute_imgedges){ } if(verbose) printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); - printf("complete time to create RGBD image: %5.5fs\n",getTime()-completeStartTime); + //printf("complete time to create RGBD image: %5.5fs\n",getTime()-completeStartTime); //getImageProbs(); if(savePath.size() != 0){ diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 8354aff0..11c65b73 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -134,6 +134,7 @@ void Model::addSuperPoints(vector & spvec, Matrix4d p, RGBDFrame* fr void Model::addAllSuperPoints(std::vector & spvec, Eigen::Matrix4d pose, boost::shared_ptr viewer){ for(unsigned int i = 0; i < frames.size(); i++){ + //printf("addAllSuperPoints: %i\n",i); addSuperPoints(spvec, pose*relativeposes[i], frames[i], modelmasks[i], viewer); } @@ -589,7 +590,7 @@ pcl::PointCloud::Ptr Model::getPCLcloud(int step, bool color){ }else{ p.b = sp.feature(0); p.g = sp.feature(1); - p.b = sp.feature(2); + p.r = sp.feature(2); } cloud_ptr->points.push_back(p); } diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 5caaf3e8..5cecabdf 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -25,7 +25,7 @@ MassRegistrationPPR2::MassRegistrationPPR2(double startreg, bool visualize){ DistanceWeightFunction2PPR2 * dwf = new DistanceWeightFunction2PPR2(); dwf->update_size = true; dwf->startreg = startreg; - dwf->debugg_print = false; + dwf->debugg_print = true; func = dwf; @@ -1363,7 +1363,7 @@ void MassRegistrationPPR2::addData(RGBDFrame* frame, ModelMask * mmask){ } } - printf("depthedge_count: %i\n",depthedge_count); + //printf("depthedge_count: %i\n",depthedge_count); if(depthedge_count > 10){ double * depthedge_ap = new double[3*depthedge_count]; double * depthedge_ai = new double[3*depthedge_count]; @@ -3086,6 +3086,7 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector poses0 = poses; for(long funcupdate=0; funcupdate < 100; ++funcupdate) { + double start_noise = func->getNoise(); if(getTime()-total_time_start > timeout){break;} //if(visualizationLvl == 2){if(use_depthedge){showEdges(poses);}std::vector Xv;for(unsigned long j = 0; j < nr_frames; j++){Xv.push_back(transformed_points[j]);}sprintf(buf,"image%5.5i.png",imgcount++);show(Xv,false,std::string(buf),imgcount); showEdges(poses);} if(visualizationLvl == 2){ @@ -3119,7 +3120,7 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vector 0){ printf("funcupdate: %i rematching: %i lala: %i\n",funcupdate,rematching,lala); printf("total_time: %5.5f\n",getTime()-total_time_start); @@ -3211,7 +3212,17 @@ MassFusionResults MassRegistrationPPR2::getTransforms(std::vectornoiseval >= kpfunc->noiseval && 0.01*func->noiseval < func->regularization){break;} //if(func->noiseval < kpfunc->noiseval && 0.01*kpfunc->noiseval < kpfunc->regularization){break;} //if(ratio > 0.99){break;} - if(func->noiseval > 10.0*func->regularization && ratio > 0.75){break;} + + double change = 1-noise_after/start_noise; + + if(visualizationLvl > 0){ + printf("start_noise: %5.5f noise_before: %5.5f noise_after: %5.5f \n found ratio: %f/%f -> %f\n",start_noise, noise_before,noise_after,noise_after,start_noise,noise_after/start_noise); + printf("check1: %10.10f > 20.0 * %10.10f -> %i\n",func->noiseval,func->regularization, func->noiseval > 20.0*func->regularization); + printf("change: %f\n",change); + } + + + if(func->noiseval > 20.0*func->regularization && fabs(change) < 0.01){break;}// && ratio < 0.9){break;} } // printf("total_time: %5.5f\n",getTime()-total_time_start); diff --git a/quasimodo/quasimodo_models/src/registration/Registration.cpp b/quasimodo/quasimodo_models/src/registration/Registration.cpp index f650de37..80a7791c 100644 --- a/quasimodo/quasimodo_models/src/registration/Registration.cpp +++ b/quasimodo/quasimodo_models/src/registration/Registration.cpp @@ -19,7 +19,7 @@ FusionResults Registration::getTransform(Eigen::MatrixXd guess){ return FusionResults(guess,0); } -void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){ +void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y, bool stop){ //printf("start show\n"); unsigned int s_nr_data = X.cols(); @@ -27,7 +27,7 @@ void Registration::show(Eigen::MatrixXd X, Eigen::MatrixXd Y){ //printf("nr datas: %i %i\n",s_nr_data,d_nr_data); -viewer->removeAllPointClouds(); + viewer->removeAllPointClouds(); pcl::PointCloud::Ptr scloud (new pcl::PointCloud); pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); @@ -40,7 +40,11 @@ viewer->removeAllPointClouds(); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scloud"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "dcloud"); //printf("pre\n"); - viewer->spin(); + if(stop){ + viewer->spin(); + }else{ + viewer->spinOnce(); + } //printf("post\n"); viewer->removeAllPointClouds(); diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp index 6da427f4..df453e71 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRandom.cpp @@ -2,6 +2,7 @@ #include #include #include +#include namespace reglib { @@ -64,15 +65,79 @@ bool RegistrationRandom::issame(FusionResults fr1, FusionResults fr2, int stepxs return mean < 20*fr1.stop; } +double getPscore(double p, double x, double y,double z, double r, pcl::PointCloud::Ptr cloud){ + double score = 0; + unsigned int nrp = cloud->points.size(); + for(unsigned int i = 0; i < nrp; i++){ + pcl::PointXYZRGBNormal po = cloud->points[i]; + double dx = po.x-x; + double dy = po.y-y; + double dz = po.z-z; + double dist = sqrt(dx*dx+dy*dy+dz*dz); + score += pow(fabs(r-dist),p); + } + return score; +} + + +double getPsphere(double p, double & x, double & y,double & z, double & r, pcl::PointCloud::Ptr cloud){ + double h = 0.00001; + + + double score = getPscore(p,x,y,z,r,cloud); + double step = 0.01; + while(step > 0.0001){ + double next_score = getPscore(p,x,y,z,r+step,cloud); + while(score > next_score){ + score = next_score; + r = r+step; + next_score = getPscore(p,x,y,z,r+step,cloud); + } + step *= 0.1; + } + + + + for(unsigned int it = 0; it < 1000; it++){ + double score_start = score; + printf("it: %10.10i -> %10.10f pos: %5.5f %5.5f %5.5f %5.5f d: ",it,score,x,y,z,r); + double dx = -(getPscore(p,x+h,y,z,r,cloud) - getPscore(p,x-h,y,z,r,cloud))/(2*h); + double dy = -(getPscore(p,x,y+h,z,r,cloud) - getPscore(p,x,y-h,z,r,cloud))/(2*h); + double dz = -(getPscore(p,x,y,z+h,r,cloud) - getPscore(p,x,y,z-h,r,cloud))/(2*h); + double dr = -(getPscore(p,x,y,z,r+h,cloud) - getPscore(p,x,y,z,r-h,cloud))/(2*h); + + printf("%5.5f %5.5f %5.5f %5.5f\n",dx,dy,dz,dr); + + double step = 0.001; + while(step > 0.00000001){ + double next_score = getPscore(p,x+step*dx,y+step*dy,z+step*dz,r+step*dr,cloud); + while(score > next_score){ + score = next_score; + x = x+step*dx; + y = y+step*dy; + z = z+step*dz; + r = r+step*dr; + next_score = getPscore(p,x+step*dx,y+step*dy,z+step*dz,r+step*dr,cloud); + } + step *= 0.1; + } + double ratio = score/score_start; + if(ratio > 0.999){break;} + } + + printf("final -> %10.10f pos: %5.5f %5.5f %5.5f %5.5f d: ",score,x,y,z,r); + return score; +} + FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ //refinement->visualizationLvl = visualizationLvl; -// refinement->viewer = viewer; -// refinement->visualizationLvl = 2; + // refinement->viewer = viewer; + // refinement->visualizationLvl = 2; unsigned int s_nr_data = src->data.cols();//std::min(int(src->data.cols()),int(500000)); unsigned int d_nr_data = dst->data.cols(); - refinement->allow_regularization = true; + refinement->allow_regularization = true; //printf("s_nr_data: %i d_nr_data: %i\n",s_nr_data,d_nr_data); int stepy = std::max(1,int(d_nr_data)/100000); @@ -101,26 +166,177 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ double s_mean_x = 0; double s_mean_y = 0; double s_mean_z = 0; - for(unsigned int i = 0; i < s_nr_data; i++){ - s_mean_x += src->data(0,i); - s_mean_y += src->data(1,i); - s_mean_z += src->data(2,i); - } - s_mean_x /= double(s_nr_data); - s_mean_y /= double(s_nr_data); - s_mean_z /= double(s_nr_data); double d_mean_x = 0; double d_mean_y = 0; double d_mean_z = 0; - for(unsigned int i = 0; i < d_nr_data; i++){ - d_mean_x += dst->data(0,i); - d_mean_y += dst->data(1,i); - d_mean_z += dst->data(2,i); + int meantype = 2; + if(meantype == 0 || meantype == 2){ + for(unsigned int i = 0; i < s_nr_data; i++){ + s_mean_x += src->data(0,i); + s_mean_y += src->data(1,i); + s_mean_z += src->data(2,i); + } + s_mean_x /= double(s_nr_data); + s_mean_y /= double(s_nr_data); + s_mean_z /= double(s_nr_data); + + for(unsigned int i = 0; i < d_nr_data; i++){ + d_mean_x += dst->data(0,i); + d_mean_y += dst->data(1,i); + d_mean_z += dst->data(2,i); + } + d_mean_x /= double(d_nr_data); + d_mean_y /= double(d_nr_data); + d_mean_z /= double(d_nr_data); + + if(meantype == 2){ + pcl::PointCloud::Ptr scloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + scloud->points.clear(); + dcloud->points.clear(); + for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = src->data(0,i) ;p.y = src->data(1,i);p.z = src->data(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);} + for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = dst->data(0,i) ;p.y = dst->data(1,i);p.z = dst->data(2,i);p.b = 0;p.g = 255;p.r = 0;dcloud->points.push_back(p);} + + +// double s_sphere_x = s_mean_x; +// double s_sphere_y = s_mean_y; +// double s_sphere_z = s_mean_z; + double s_sphere_r = 0; + double sscore = getPsphere(1.0, s_mean_x,s_mean_y,s_mean_z,s_sphere_r,scloud); + + +// double d_sphere_x = d_mean_x; +// double d_sphere_y = d_mean_y; +// double d_sphere_z = d_mean_z; + double d_sphere_r = 0; + double dscore = getPsphere(1.0, d_mean_x,d_mean_y,d_mean_z,d_sphere_r,dcloud); + } + + }else if(meantype == 1){ + std::vector s_xvec; + std::vector s_yvec; + std::vector s_zvec; + s_xvec.resize(s_nr_data); + s_yvec.resize(s_nr_data); + s_zvec.resize(s_nr_data); + for(unsigned int i = 0; i < s_nr_data; i++){ + s_xvec[i] = src->data(0,i); + s_yvec[i] = src->data(1,i); + s_zvec[i] = src->data(2,i); + } + + std::sort (s_xvec.begin(), s_xvec.end()); + std::sort (s_yvec.begin(), s_yvec.end()); + std::sort (s_zvec.begin(), s_zvec.end()); + + std::vector d_xvec; + std::vector d_yvec; + std::vector d_zvec; + d_xvec.resize(d_nr_data); + d_yvec.resize(d_nr_data); + d_zvec.resize(d_nr_data); + for(unsigned int i = 0; i < d_nr_data; i++){ + d_xvec[i] = dst->data(0,i); + d_yvec[i] = dst->data(1,i); + d_zvec[i] = dst->data(2,i); + } + + std::sort (d_xvec.begin(), d_xvec.end()); + std::sort (d_yvec.begin(), d_yvec.end()); + std::sort (d_zvec.begin(), d_zvec.end()); + + int src_mid_ind = s_nr_data/2; + int dst_mid_ind = d_nr_data/2; + + s_mean_x = s_xvec[src_mid_ind]; + s_mean_y = s_xvec[src_mid_ind]; + s_mean_z = s_xvec[src_mid_ind]; + + d_mean_x = d_xvec[dst_mid_ind]; + d_mean_y = d_xvec[dst_mid_ind]; + d_mean_z = d_xvec[dst_mid_ind]; + +// printf("s mean: %10.10f %10.10f %10.10f\n",s_mean_x,s_mean_y,s_mean_z); +// printf("s median: %10.10f %10.10f %10.10f\n",s_xvec[src_mid_ind],s_yvec[src_mid_ind],s_zvec[src_mid_ind]); +// printf("d mean: %10.10f %10.10f %10.10f\n",d_mean_x,d_mean_y,d_mean_z); +// printf("d median: %10.10f %10.10f %10.10f\n",d_xvec[dst_mid_ind],d_yvec[dst_mid_ind],d_zvec[dst_mid_ind]); } - d_mean_x /= double(d_nr_data); - d_mean_y /= double(d_nr_data); - d_mean_z /= double(d_nr_data); + + + + +// viewer->removeAllPointClouds(); +// pcl::PointCloud::Ptr scloud (new pcl::PointCloud); +// pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + +// scloud->points.clear(); +// dcloud->points.clear(); +// for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = src->data(0,i) ;p.y = src->data(1,i);p.z = src->data(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);} +// for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = dst->data(0,i)+2;p.y = dst->data(1,i);p.z = dst->data(2,i);p.b = 0;p.g = 255;p.r = 0;dcloud->points.push_back(p);} +// viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); +// viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); +// viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scloud"); +// viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "dcloud"); + +// double s_sphere_x = s_mean_x; +// double s_sphere_y = s_mean_y; +// double s_sphere_z = s_mean_z; +// double s_sphere_r = 0; +// double sscore = getPsphere(1.0, s_sphere_x,s_sphere_y,s_sphere_z,s_sphere_r,scloud); + + +// double d_sphere_x = d_mean_x+2; +// double d_sphere_y = d_mean_y; +// double d_sphere_z = d_mean_z; +// double d_sphere_r = 0; +// double dscore = getPsphere(1.0, d_sphere_x,d_sphere_y,d_sphere_z,d_sphere_r,dcloud); + +// pcl::PointXYZRGB smin; +// smin.x = s_mean_x; +// smin.y = s_mean_y; +// smin.z = s_mean_z; + +// pcl::PointXYZRGB dmin; +// dmin.x = d_mean_x+2; +// dmin.y = d_mean_y; +// dmin.z = d_mean_z; + +// pcl::PointXYZRGB s_sphere_p; +// s_sphere_p.x = s_sphere_x; +// s_sphere_p.y = s_sphere_y; +// s_sphere_p.z = s_sphere_z; + +// pcl::PointXYZRGB d_sphere_p; +// d_sphere_p.x = d_sphere_x; +// d_sphere_p.y = d_sphere_y; +// d_sphere_p.z = d_sphere_z; + +// pcl::PointXYZRGB smed; +// smed.x = s_xvec[src_mid_ind]; +// smed.y = s_yvec[src_mid_ind]; +// smed.z = s_zvec[src_mid_ind]; + +// pcl::PointXYZRGB dmed; +// dmed.x = d_xvec[dst_mid_ind]+2; +// dmed.y = d_yvec[dst_mid_ind]; +// dmed.z = d_zvec[dst_mid_ind]; + +// viewer->addSphere (smin, 0.01, 1, 0, 0, "smin"); +// viewer->addSphere (dmin, 0.01, 1, 0, 0, "dmin"); + +// viewer->addSphere (smed, 0.01, 0, 0, 1, "smed"); +// viewer->addSphere (dmed, 0.01, 0, 0, 1, "dmed"); + +// viewer->addSphere (s_sphere_p, s_sphere_r, 0, 1, 1, "s_sphere"); +// viewer->addSphere (d_sphere_p, d_sphere_r, 0, 1, 1, "d_sphere"); + +// //printf("pre\n"); +// viewer->spin(); +// //printf("post\n"); +// viewer->removeAllPointClouds(); +// } + Eigen::Affine3d Ymean = Eigen::Affine3d::Identity(); Ymean(0,3) = d_mean_x; @@ -142,19 +358,29 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ double sumtimeSum = 0; double sumtimeOK = 0; - refinement->viewer = viewer; + refinement->viewer = viewer; refinement->visualizationLvl = 0; std::vector< double > rxs; std::vector< double > rys; std::vector< double > rzs; - double step = 0.1+2.0*M_PI/5; - for(double rx = 0; rx < 2.0*M_PI; rx += step){ - for(double ry = 0; ry < 2.0*M_PI; ry += step){ - for(double rz = 0; rz < 2.0*M_PI; rz += step){ - rxs.push_back(rx); - rys.push_back(ry); - rzs.push_back(rz); + double step = 0.1+2.0*M_PI/7; + // for(double rx = 0; rx < 2.0*M_PI; rx += step){ + // for(double ry = 0; ry < 2.0*M_PI; ry += step){ + // for(double rz = 0; rz < 2.0*M_PI; rz += step){ + // rxs.push_back(rx); + // rys.push_back(ry); + // rzs.push_back(rz); + // } + // } + // } + int steps = 6; + for(double rx = 0; rx < steps; rx ++){ + for(double ry = 0; ry < steps; ry++){ + for(double rz = 0; rz < steps; rz++){ + rxs.push_back(2.0 * M_PI * (rx/double(steps+1))); + rys.push_back(2.0 * M_PI * (ry/double(steps+1))); + rzs.push_back(2.0 * M_PI * (rz/double(steps+1))); } } } @@ -166,7 +392,8 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ std::vector fr_X; fr_X.resize(nr_r); - #pragma omp parallel for num_threads(8) + //refinement->visualizationLvl = visualizationLvl; + #pragma omp parallel for num_threads(8) for(unsigned int r = 0; r < nr_r; r++){ //printf("registering: %i / %i\n",r+1,nr_r); double start = getTime(); @@ -177,15 +404,15 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ Eigen::Affine3d randomrot = Eigen::Affine3d::Identity(); randomrot = Eigen::AngleAxisd(rxs[r], Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(rys[r], Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rzs[r], Eigen::Vector3d::UnitZ()); + Eigen::AngleAxisd(rys[r], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rzs[r], Eigen::Vector3d::UnitZ()); Eigen::Affine3d current_guess = Ymean*randomrot*Xmean.inverse();//*Ymean; FusionResults fr = refinement->getTransform(current_guess.matrix()); //fr_X[r] = refinement->getTransform(current_guess.matrix()); - #pragma omp critical +#pragma omp critical { fr_X[r] = fr; @@ -195,28 +422,28 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ sumtimeSum += stoptime-start; sumtimeOK++; } - // bool exists2 = false; - // for(unsigned int ax = 0; ax < fr_X.size(); ax++){ - // if(issame(fr, fr_X[ax],stepxsmall)){exists2 = true; break;} - // } - // if(!exists2){fr_X.push_back(fr);} + // bool exists2 = false; + // for(unsigned int ax = 0; ax < fr_X.size(); ax++){ + // if(issame(fr, fr_X[ax],stepxsmall)){exists2 = true; break;} + // } + // if(!exists2){fr_X.push_back(fr);} } } FusionResults fr = FusionResults(); - refinement->allow_regularization = false; + refinement->allow_regularization = false; int tpbef = refinement->target_points; - int mul = 1; - for(int tp = 500; tp <= 16000; tp *= 2){ - std::sort (fr_X.begin(), fr_X.end(), compareFusionResults); + int mul = 2; + for(int tp = 500; tp <= 16000; tp *= 2){ + std::sort (fr_X.begin(), fr_X.end(), compareFusionResults); refinement->target_points = tp; - unsigned int nr_X = fr_X.size()/mul; - #pragma omp parallel for num_threads(8) + unsigned int nr_X = fr_X.size()/mul; +#pragma omp parallel for num_threads(8) for(unsigned int ax = 0; ax < nr_X; ax++){ fr_X[ax] = refinement->getTransform(fr_X[ax].guess); } @@ -230,28 +457,28 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ } } } - mul *= 2; + mul *= 2; } -// for(int tp = 500; tp <= 1000; tp *= 2){ -// refinement->target_points = tp; - -// unsigned int nr_X = fr_X.size(); -// //#pragma omp parallel for num_threads(8) -// for(unsigned int ax = 0; ax < nr_X; ax++){ -// fr_X[ax] = refinement->getTransform(fr_X[ax].guess); -// } - -// for(unsigned int ax = 0; ax < fr_X.size(); ax++){ -// for(unsigned int bx = ax+1; bx < fr_X.size(); bx++){ -// if(issame(fr_X[bx], fr_X[ax],stepxsmall)){ -// fr_X[bx] = fr_X.back(); -// fr_X.pop_back(); -// bx--; -// } -// } -// } -// } + // for(int tp = 500; tp <= 1000; tp *= 2){ + // refinement->target_points = tp; + + // unsigned int nr_X = fr_X.size(); + // //#pragma omp parallel for num_threads(8) + // for(unsigned int ax = 0; ax < nr_X; ax++){ + // fr_X[ax] = refinement->getTransform(fr_X[ax].guess); + // } + + // for(unsigned int ax = 0; ax < fr_X.size(); ax++){ + // for(unsigned int bx = ax+1; bx < fr_X.size(); bx++){ + // if(issame(fr_X[bx], fr_X[ax],stepxsmall)){ + // fr_X[bx] = fr_X.back(); + // fr_X.pop_back(); + // bx--; + // } + // } + // } + // } refinement->visualizationLvl = visualizationLvl; std::sort (fr_X.begin(), fr_X.end(), compareFusionResults); @@ -262,15 +489,17 @@ FusionResults RegistrationRandom::getTransform(Eigen::MatrixXd guess){ } - if(visualizationLvl > 0){ - refinement->visualizationLvl = 2; - refinement->target_points = 10000; - for(unsigned int ax = 0; ax < fr_X.size() && ax < 5; ax++){ - printf("%i -> %f\n",ax,fr_X[ax].score); - refinement->getTransform(fr_X[ax].guess); - } - refinement->visualizationLvl = 0; - } + if(visualizationLvl > 0){ + refinement->allow_regularization = true; + refinement->visualizationLvl = visualizationLvl; + refinement->target_points = 1000000; + refinement->maxtime = 10000; + for(unsigned int ax = 0; ax < fr_X.size() && ax < 20; ax++){ + printf("%i -> %f\n",ax,fr_X[ax].score); + refinement->getTransform(fr_X[ax].guess); + } + refinement->visualizationLvl = 0; + } refinement->target_points = tpbef; diff --git a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp index bb7a57c2..78b78686 100644 --- a/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp +++ b/quasimodo/quasimodo_models/src/registration/RegistrationRefinement.cpp @@ -25,13 +25,13 @@ RegistrationRefinement::RegistrationRefinement(){ visualizationLvl = 1; - target_points = 250; - allow_regularization = true; - maxtime = 9999999; + target_points = 250; + allow_regularization = true; + maxtime = 9999999; -// func = new DistanceWeightFunction2PPR2(); -// func->startreg = 0.1; -// func->debugg_print = false; + // func = new DistanceWeightFunction2PPR2(); + // func->startreg = 0.1; + // func->debugg_print = false; } RegistrationRefinement::~RegistrationRefinement(){ //if(func != 0){delete func; func = 0;} @@ -97,7 +97,7 @@ FusionResults RegistrationRefinement::getTransform(Eigen::MatrixXd guess){ unsigned int s_nr_data = src->data.cols(); - int stepx = std::max(1,int(s_nr_data)/target_points); + int stepx = std::max(1,int(s_nr_data)/target_points); double stop = 0.00001; @@ -149,20 +149,42 @@ FusionResults RegistrationRefinement::getTransform(Eigen::MatrixXd guess){ double score = 0; stop = 99999; - if(visualizationLvl >= 3){show(X,Y);} + if(visualizationLvl >= 3){show(X,Y,true);} + + // //printf("X: %i %i Y: %i %i\n",X.cols(), X.rows(),Y.cols(), Y.rows()); + //double startTest = getTime(); + //std::vector ret_indexes1(1); + //std::vector out_dists_sqr1(1); + //nanoflann::KNNResultSet resultSet1(1); + //resultSet1.init(&ret_indexes1[0], &out_dists_sqr1[0] ); + //double qp1 [3]; + //nanoflann::SearchParams sp1 = nanoflann::SearchParams(10); + + //for(unsigned long it = 0; true; it++){ + // qp1[0] = 0.001*(rand()%1000); + // qp1[1] = 0.001*(rand()%1000); + // qp1[2] = 0.001*(rand()%1000); + // trees3d->findNeighbors(resultSet1, qp1, sp1); + // int mid = ret_indexes1[0]; + // if(it % 1000){ + // printf("mps: %15.15f\n",double(it)/(getTime()-startTest)); + // } + //} - //printf("X: %i %i Y: %i %i\n",X.cols(), X.rows(),Y.cols(), Y.rows()); - double start = getTime(); -bool timestopped = false; + + double start = getTime(); + long matches = 0; + bool timestopped = false; /// ICP for(int funcupdate=0; funcupdate < 100; ++funcupdate) { if( (getTime()-start) > maxtime ){timestopped = true; break;} for(int rematching=0; rematching < 100; ++rematching) { if( (getTime()-start) > maxtime ){timestopped = true; break;} -//#pragma omp parallel for + + //#pragma omp parallel for for(unsigned int i=0; i< xcols; ++i) { std::vector ret_indexes(1); std::vector out_dists_sqr(1); @@ -173,9 +195,10 @@ bool timestopped = false; trees3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); matchid[i] = ret_indexes[0]; } + matches += xcols; /// Find closest point -//#pragma omp parallel for + //#pragma omp parallel for for(unsigned int i=0; i< xcols; ++i) { int id = matchid[i]; Qn.col(i) = N.col(id); @@ -187,33 +210,33 @@ bool timestopped = false; if( (getTime()-start) > maxtime ){timestopped = true; break;} /// Compute weights switch(type) { - case PointToPoint: {residuals = X-Qp;} break; - case PointToPlane: { - residuals = Eigen::MatrixXd::Zero(1, xcols); - for(unsigned int i=0; icomputeModel(residuals);} break; - case PointToPlane: {func->computeModel(residuals);} break; - default: {printf("type not set\n");} break; + case PointToPoint: {func->computeModel(residuals);} break; + case PointToPlane: {func->computeModel(residuals);} break; + default: {printf("type not set\n");} break; } for(int rematching2=0; rematching2 < 3; ++rematching2) { if( (getTime()-start) > maxtime ){timestopped = true; break;} if(rematching2 != 0){ -//#pragma omp parallel for + //#pragma omp parallel for for(unsigned int i=0; i< xcols; ++i) { std::vector ret_indexes(1); std::vector out_dists_sqr(1); @@ -224,9 +247,10 @@ bool timestopped = false; trees3d->findNeighbors(resultSet, qp, nanoflann::SearchParams(10)); matchid[i] = ret_indexes[0]; } + matches += xcols; /// Find closest point -//#pragma omp parallel for + //#pragma omp parallel for for(unsigned int i=0; i< xcols; ++i) { int id = matchid[i]; Qn.col(i) = N.col(id); @@ -239,41 +263,45 @@ bool timestopped = false; if( (getTime()-start) > maxtime ){timestopped = true; break;} if(inner != 0){ switch(type) { - case PointToPoint: {residuals = X-Qp;} break; - case PointToPlane: { - residuals = Eigen::MatrixXd::Zero(1, xcols); - for(unsigned int i=0; i< xcols; ++i) { - float dx = X(0,i)-Qp(0,i); - float dy = X(1,i)-Qp(1,i); - float dz = X(2,i)-Qp(2,i); - float qx = Qn(0,i); - float qy = Qn(1,i); - float qz = Qn(2,i); - float di = qx*dx + qy*dy + qz*dz; - residuals(0,i) = di; - } - }break; - default: {printf("type not set\n");} break; + case PointToPoint: {residuals = X-Qp;} break; + case PointToPlane: { + residuals = Eigen::MatrixXd::Zero(1, xcols); + for(unsigned int i=0; i< xcols; ++i) { + float dx = X(0,i)-Qp(0,i); + float dy = X(1,i)-Qp(1,i); + float dz = X(2,i)-Qp(2,i); + float qx = Qn(0,i); + float qy = Qn(1,i); + float qz = Qn(2,i); + float di = qx*dx + qy*dy + qz*dz; + residuals(0,i) = di; + } + }break; + default: {printf("type not set\n");} break; } - for(unsigned int i=0; igetProbs(residuals); } break; - case PointToPlane: { - W = func->getProbs(residuals); - for(unsigned int i=0; i 0.0);} - } break; - default: {printf("type not set\n");} break; + case PointToPoint: {W = func->getProbs(residuals); } break; + case PointToPlane: { + W = func->getProbs(residuals); + for(unsigned int i=0; i 0.0); + W(i) = W(i)*float((Xn(0,i)*Qn(0,i) + Xn(1,i)*Qn(1,i) + Xn(2,i)*Qn(2,i)) > 0.0); + } + } break; + default: {printf("type not set\n");} break; } Wold = W; //Normalizing weights has an effect simmilar to one to one matching //in that it reduces the effect of border points if(normalize_matchweights){ -// for(unsigned int i=0; i < ycols; ++i) { total_dweight[i] = 0.0000001;}//Reset to small number to avoid division by zero -// for(unsigned int i=0; i< xcols; ++i) { total_dweight[matchid[i]] += W(i);} -// for(unsigned int i=0; i< xcols; ++i) { W(i) = W(i)*(W(i)/total_dweight[matchid[i]]);} + // for(unsigned int i=0; i < ycols; ++i) { total_dweight[i] = 0.0000001;}//Reset to small number to avoid division by zero + // for(unsigned int i=0; i< xcols; ++i) { total_dweight[matchid[i]] += W(i);} + // for(unsigned int i=0; i< xcols; ++i) { W(i) = W(i)*(W(i)/total_dweight[matchid[i]]);} for(unsigned int i=0; i < ycols; ++i) { total_dweight[i] = 99990.0000001;}//Reset to small number to avoid division by zero for(unsigned int i=0; i< xcols; ++i) { total_dweight[matchid[i]] = std::min(total_dweight[matchid[i]],residuals.col(i).norm());} @@ -284,41 +312,46 @@ bool timestopped = false; W = W.array()*rangeW.array()*rangeW.array(); - if(visualizationLvl >= 4){ - //printf("start show\n"); - unsigned int s_nr_data = X.cols(); - unsigned int d_nr_data = Y.cols(); - //printf("nr datas: %i %i\n",s_nr_data,d_nr_data); - - - viewer->removeAllPointClouds(); - pcl::PointCloud::Ptr scloud (new pcl::PointCloud); - pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); - - scloud->points.clear(); - dcloud->points.clear(); - for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);} - for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);} - viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); - viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scloud"); - viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "dcloud"); - //printf("pre\n"); - viewer->spin(); - //printf("post\n"); - viewer->removeAllPointClouds(); - } + if(visualizationLvl == 3){ + show(X,Y,false); + + }else if(visualizationLvl >= 4){ + //printf("start show\n"); + unsigned int s_nr_data = X.cols(); + unsigned int d_nr_data = Y.cols(); + //printf("nr datas: %i %i\n",s_nr_data,d_nr_data); + + + viewer->removeAllPointClouds(); + pcl::PointCloud::Ptr scloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + + scloud->points.clear(); + dcloud->points.clear();/* + for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);} + for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);}*/ + for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 255.0*Wold(i);p.g = 255.0*Wold(i);p.r = 255.0*Wold(i);scloud->points.push_back(p);} + for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0; p.g = 0; p.r = 255; dcloud->points.push_back(p);} + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "dcloud"); + //printf("pre\n"); + viewer->spinOnce(); + //printf("post\n"); + viewer->removeAllPointClouds(); + } switch(type) { - case PointToPoint: { - pcl::TransformationFromCorrespondences tfc1; - for(unsigned int c = 0; c < X.cols(); c++){tfc1.add(Eigen::Vector3f(X(0,c), X(1,c),X(2,c)),Eigen::Vector3f(Qp(0,c),Qp(1,c),Qp(2,c)),W(c));} - Eigen::Affine3d rot = tfc1.getTransformation().cast(); - X = rot*X; - Xn = rot.rotation()*Xn; - }break; - case PointToPlane: {point_to_plane2(X, Xn, Qp, Qn, W);}break; - default: {printf("type not set\n"); } break; + case PointToPoint: { + pcl::TransformationFromCorrespondences tfc1; + for(unsigned int c = 0; c < X.cols(); c++){tfc1.add(Eigen::Vector3f(X(0,c), X(1,c),X(2,c)),Eigen::Vector3f(Qp(0,c),Qp(1,c),Qp(2,c)),W(c));} + Eigen::Affine3d rot = tfc1.getTransformation().cast(); + X = rot*X; + Xn = rot.rotation()*Xn; + }break; + case PointToPlane: {point_to_plane2(X, Xn, Qp, Qn, W);}break; + default: {printf("type not set\n"); } break; } @@ -329,9 +362,9 @@ bool timestopped = false; score = Wold.sum()/(func->getNoise()*float(xcols)); -// printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise()); -// printf("X: %i %i Y: %i %i\n",X.cols(), X.rows(),Y.cols(), Y.rows()); -// show(X,Y); + // printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise()); + // printf("X: %i %i Y: %i %i\n",X.cols(), X.rows(),Y.cols(), Y.rows()); + // show(X,Y); double stop1 = (X-Xo1).colwise().norm().mean(); Xo1 = X; if(stop1 < stop) break; @@ -354,9 +387,40 @@ bool timestopped = false; if(fabs(1.0 - noise_after/noise_before) < 0.01){break;} } + // printf("mps: %15.15f ratio: %15.15f\n",double(matches)/(getTime()-start),(double(matches)/(getTime()-start))/16000.0); + //exit(0); + //printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise()); - if(visualizationLvl >= 2){printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise()); show(X,Y);} + if(visualizationLvl == 2){printf("xcols: %i stepx: %i stop: %f noise: %f\n",xcols,stepx,stop,func->getNoise()); show(X,Y);} + if(visualizationLvl >= 4){ + printf("visualizationLvl: %i\n",visualizationLvl); + //printf("start show\n"); + unsigned int s_nr_data = X.cols(); + unsigned int d_nr_data = Y.cols(); + //printf("nr datas: %i %i\n",s_nr_data,d_nr_data); + + + viewer->removeAllPointClouds(); + pcl::PointCloud::Ptr scloud (new pcl::PointCloud); + pcl::PointCloud::Ptr dcloud (new pcl::PointCloud); + + scloud->points.clear(); + dcloud->points.clear();/* + for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 0;p.g = 255;p.r = 0;scloud->points.push_back(p);} + for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0;p.g = 0;p.r = 255;dcloud->points.push_back(p);}*/ + for(unsigned int i = 0; i < s_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = X(0,i);p.y = X(1,i);p.z = X(2,i);p.b = 255.0*Wold(i);p.g = 255.0*Wold(i);p.r = 255.0*Wold(i);scloud->points.push_back(p);} + for(unsigned int i = 0; i < d_nr_data; i++){pcl::PointXYZRGBNormal p;p.x = Y(0,i);p.y = Y(1,i);p.z = Y(2,i);p.b = 0; p.g = 0; p.r = 255; dcloud->points.push_back(p);} + viewer->addPointCloud (scloud, pcl::visualization::PointCloudColorHandlerRGBField(scloud), "scloud"); + viewer->addPointCloud (dcloud, pcl::visualization::PointCloudColorHandlerRGBField(dcloud), "dcloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "scloud"); + viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "dcloud"); + //printf("pre\n"); + viewer->spin(); + //printf("post\n"); + viewer->removeAllPointClouds(); + } + pcl::TransformationFromCorrespondences tfc; tfc.reset(); From 0bea40ecb7216b5d5e3b65624650941990e9e0e7 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Sat, 19 Nov 2016 08:43:14 +0100 Subject: [PATCH 213/255] ... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 4 +- strands_sweep_registration/include/Camera.h | 41 +++++++++++ strands_sweep_registration/include/Frame.h | 39 +++++++++++ .../include/PixelFunction.h | 20 ++++++ .../include/ProblemFrameConnection.h | 41 +++++++++++ .../include/RobotContainer.h | 68 +++++++++++++++++++ strands_sweep_registration/include/Sweep.h | 25 +++++++ .../include/camera_parameters.h | 57 ++++++++++++++++ .../include/pair3DError.h | 41 +++++++++++ strands_sweep_registration/include/util.h | 16 +++++ 10 files changed, 350 insertions(+), 2 deletions(-) create mode 100644 strands_sweep_registration/include/Camera.h create mode 100644 strands_sweep_registration/include/Frame.h create mode 100644 strands_sweep_registration/include/PixelFunction.h create mode 100644 strands_sweep_registration/include/ProblemFrameConnection.h create mode 100644 strands_sweep_registration/include/RobotContainer.h create mode 100644 strands_sweep_registration/include/Sweep.h create mode 100644 strands_sweep_registration/include/camera_parameters.h create mode 100644 strands_sweep_registration/include/pair3DError.h create mode 100644 strands_sweep_registration/include/util.h diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index e0f98774..f6c40c70 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -338,8 +338,8 @@ void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > massregmod->timeout = 1200; massregmod->viewer = viewer; massregmod->visualizationLvl = debugg > 1; - massregmod->maskstep = 11;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massregmod->nomaskstep = 11;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massregmod->maskstep = 4;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massregmod->nomaskstep = 4;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massregmod->nomask = true; massregmod->stopval = 0.0001; diff --git a/strands_sweep_registration/include/Camera.h b/strands_sweep_registration/include/Camera.h new file mode 100644 index 00000000..702561a4 --- /dev/null +++ b/strands_sweep_registration/include/Camera.h @@ -0,0 +1,41 @@ +#ifndef Camera_H_ +#define Camera_H_ + +#include +#include +#include + +#include "PixelFunction.h" + +class Camera { + public: + float fx; + float fy; + float cx; + float cy; + + int width; + int height; + + int version; + + PixelFunction ** pixelsFunctions; + + unsigned int coefs_degree; + unsigned int coefs_width; + unsigned int coefs_height; + double * multiplierCoeffs; + + Camera( float fx_, float fy_, float cx_, float cy_, int width_, int height_); + virtual ~Camera(); + + virtual double * getCoeffs(unsigned int w, unsigned int h); + virtual double getGridMultiplier(unsigned int w, unsigned int h, double z); + virtual double getMultiplier(double w, double h, double z); + virtual void save(std::string path); + virtual void load(std::string path); + virtual void show(); + virtual void print(); +}; + +#endif diff --git a/strands_sweep_registration/include/Frame.h b/strands_sweep_registration/include/Frame.h new file mode 100644 index 00000000..1bff8978 --- /dev/null +++ b/strands_sweep_registration/include/Frame.h @@ -0,0 +1,39 @@ +#ifndef Frame_H_ +#define Frame_H_ + +#include + +#include "Camera.h" + +#include "opencv2/core/core.hpp" +#include "opencv2/highgui/highgui.hpp" +#include "opencv/cv.hpp" +#include "opencv/highgui.h" + +#include + +class Camera; + +class Frame +{ + public: + Camera * camera; + + int id; + int framepos; + + int featuretype; + std::vector keypoints; + cv::Mat descriptors; + std::vector keypoint_depth; + + std::vector keypoint_location; + + Frame(Camera * camera_); + Frame(Camera * camera_, float * rgb_data_, float * depth_data_); + Frame(Camera * camera_, std::vector k, std::vector depth, cv::Mat d); // initialize from ORB features driectly + ~Frame(); + + void recalculateFullPoints(); +}; +#endif diff --git a/strands_sweep_registration/include/PixelFunction.h b/strands_sweep_registration/include/PixelFunction.h new file mode 100644 index 00000000..74c55dba --- /dev/null +++ b/strands_sweep_registration/include/PixelFunction.h @@ -0,0 +1,20 @@ +#ifndef PixelFunction_H_ +#define PixelFunction_H_ +#include + +class PixelFunction +{ + public: + float r_mul; + float g_mul; + float b_mul; + float d_mul; + PixelFunction(); + ~PixelFunction(); + + void getValues(float * pixel); + void update(std::vector data, float bias = 2, float random = 0, float step = 1); + void addOutput( std::vector & pixeldata_char, std::vector & pixeldata_counter); + void load(char * buffer,int & pos); +}; +#endif diff --git a/strands_sweep_registration/include/ProblemFrameConnection.h b/strands_sweep_registration/include/ProblemFrameConnection.h new file mode 100644 index 00000000..77302b1b --- /dev/null +++ b/strands_sweep_registration/include/ProblemFrameConnection.h @@ -0,0 +1,41 @@ +#ifndef ProblemFrameConnection_H_ +#define ProblemFrameConnection_H_ + +#include "pair3DError.h" +#include "Frame.h" + +class ProblemFrameConnection { + public: + Frame * src; + Frame * dst; + + double * params; + double * src_variable; + double * dst_variable; + + std::vector< Eigen::Vector3f > full_src_points; + std::vector< Eigen::Vector3f > full_dst_points; + std::vector< float > full_noisemul; + + + std::vector< Eigen::Vector3f > src_points; + std::vector< Eigen::Vector3f > dst_points; + std::vector< float > noisemul; + + std::vector< float > possible_matches_fdistance; + std::vector< float > possible_matches_edistance; + std::vector src_possible_matches_id; + std::vector dst_possible_matches_id; + + std::vector src_matches; + std::vector dst_matches; + + ProblemFrameConnection(ceres::Problem & problem, Frame * src_, Frame * dst_, double * shared_params, double * src_variable_, double * dst_variable_, float weight = 1, bool show = false); + void addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions, double hubersize = 0.1); + void addMatchesToProblem(ceres::Problem & problem, float weight = 1); + void findPossibleMatches(float di = 1, float pi = 0, Eigen::Matrix4f pose = Eigen::Matrix4f::Identity()); + void recalculatePoints(); + ~ProblemFrameConnection(); +}; + +#endif diff --git a/strands_sweep_registration/include/RobotContainer.h b/strands_sweep_registration/include/RobotContainer.h new file mode 100644 index 00000000..63690178 --- /dev/null +++ b/strands_sweep_registration/include/RobotContainer.h @@ -0,0 +1,68 @@ +#ifndef RobotContainer_H_ +#define RobotContainer_H_ + +#include +#include "Frame.h" +#include "Camera.h" +#include "Sweep.h" + +#include "pair3DError.h" +#include "ProblemFrameConnection.h" + +#include +#include +#include + +class RobotContainer +{ + public: + + unsigned int width; + unsigned int height; + + unsigned int gx; + unsigned int todox; + unsigned int gy; + unsigned int todoy; + unsigned int ** inds; + + float * rgb; + float * depth; + + double * shared_params; + + double *** poses; + + Camera * camera; + std::vector sweeps; + std::vector alignedSweep; + + + std::vector paths; + + bool viewer_initialized; + boost::shared_ptr viewer; + + RobotContainer(unsigned int gx_,unsigned int todox_,unsigned int gy_,unsigned int todoy_); + ~RobotContainer(); + + void initializeCamera(double fx, double fy, double cx, double cy, unsigned int w, unsigned int h); + void addToTraining(std::string path); // this loads a whole sweep and extracts ORB features + bool addToTrainingORBFeatures(std::string path); // this loads previously comptued ORB features + + std::vector runInitialTraining(); + void refineTraining(); + std::vector train(); + bool isCalibrated(); + + std::vector alignAndStoreSweeps(); + + void saveAllSweeps(std::string path); + + void show(); + +private: + void saveSweep(Sweep*, std::string path); + +}; +#endif diff --git a/strands_sweep_registration/include/Sweep.h b/strands_sweep_registration/include/Sweep.h new file mode 100644 index 00000000..9cc8c5b3 --- /dev/null +++ b/strands_sweep_registration/include/Sweep.h @@ -0,0 +1,25 @@ +#ifndef Sweep_H_ +#define Sweep_H_ +#include +#include +#include "Frame.h" + +class Sweep +{ + public: + std::string idtag; + std::string xmlpath; + + int width; + int height; + Frame *** frames; + Eigen::Matrix4f ** poses; + + Sweep(int width_, int height_, std::vector framesvec); + Sweep(); + virtual ~Sweep(); + + Eigen::Matrix4f align(Sweep * sweep, float threshold = 0.02, int ransac_iter = 20000, int nr_points = 3); + std::vector getPoseVector(); +}; +#endif diff --git a/strands_sweep_registration/include/camera_parameters.h b/strands_sweep_registration/include/camera_parameters.h new file mode 100644 index 00000000..cdc5f1cf --- /dev/null +++ b/strands_sweep_registration/include/camera_parameters.h @@ -0,0 +1,57 @@ +#ifndef __CAMERA_PARAMETERS__ +#define __CAMERA_PARAMETERS__ + +#include + +class CameraParameters +{ + public: + static const CameraParameters & get(double fx = 0.0, double fy = 0.0, double cx = 0.0, double cy = 0.0, unsigned int width = 0, unsigned int height = 0) + { + static const CameraParameters instance(fx, fy, cx, cy, width, height); + return instance; + } + const double & fx() const + { + return fx_; + } + const double & fy() const + { + return fy_; + } + const double & cx() const + { + return cx_; + } + const double & cy() const + { + return cy_; + } + const unsigned int & width() const + { + return width_; + } + const unsigned int & height() const + { + return height_; + } + private: + CameraParameters(double fx, double fy, double cx, double cy, unsigned int width, unsigned int height) + : fx_(fx), + fy_(fy), + cx_(cx), + cy_(cy), + width_(width), + height_(height) + { + assert(fx_ > 0.0 && fy_ > 0.0 && cx_ > 0.0 && cy_ > 0.0 && height_ > 0 && width_ > 0 && "Please set the camera parameters!"); + } + const double fx_; + const double fy_; + const double cx_; + const double cy_; + const unsigned int width_; + const unsigned int height_; +}; + +#endif diff --git a/strands_sweep_registration/include/pair3DError.h b/strands_sweep_registration/include/pair3DError.h new file mode 100644 index 00000000..238ce2cd --- /dev/null +++ b/strands_sweep_registration/include/pair3DError.h @@ -0,0 +1,41 @@ +#ifndef pair3DError_H_ +#define pair3DError_H_ +#include "ceres/ceres.h" +#include "gflags/gflags.h" +#include "glog/logging.h" +#include "ceres/rotation.h" +#include "ceres/iteration_callback.h" + +#include "util.h" + + + +using ceres::NumericDiffCostFunction; +using ceres::SizedCostFunction; +using ceres::CENTRAL; +using ceres::AutoDiffCostFunction; +using ceres::CostFunction; +using ceres::Problem; +using ceres::Solver; +using ceres::Solve; +using ceres::Solve; + +class pair3DError : public SizedCostFunction<3, 6, 6, 4> { + public: + static bool optimizeCameraParams; + pair3DError(double sw, double sh, double sz,double dw, double dh, double dz, double weight); + ~pair3DError(); + bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const ; + + int id; + double sw; + double sh; + double sz; + double dw; + double dh; + double dz; + double weight; + //bool optimizeCameraParams; + double information; +}; +#endif diff --git a/strands_sweep_registration/include/util.h b/strands_sweep_registration/include/util.h new file mode 100644 index 00000000..496a80ed --- /dev/null +++ b/strands_sweep_registration/include/util.h @@ -0,0 +1,16 @@ +#ifndef myutil_H_ +#define myutil_H_ + +// PCL specific includes +#include +#include +#include +#include +#include + +int popcount_lauradoux(uint64_t *buf, uint32_t size); +template Eigen::Matrix getMat(const T* const camera, int mode = 0); +template void transformPoint(const T* const camera, T * point, int mode = 0); +void getMat(const double* const camera, double * mat); + +#endif From 0ad303a774d70920f00d5f4d25d7098d3e8f96ed Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 21 Nov 2016 13:45:13 +0100 Subject: [PATCH 214/255] Added a new method for constructing soma_llsd scenes --- .../quasimodo_conversions/CMakeLists.txt | 11 ++- .../quasimodo_conversions/conversions.h | 9 +++ .../src/quasimodo_conversions/conversions.cpp | 79 +++++++++++++++++++ 3 files changed, 98 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_conversions/CMakeLists.txt b/quasimodo/quasimodo_conversions/CMakeLists.txt index e8e28d43..cefc5a75 100644 --- a/quasimodo/quasimodo_conversions/CMakeLists.txt +++ b/quasimodo/quasimodo_conversions/CMakeLists.txt @@ -6,11 +6,18 @@ add_definitions(-std=c++11) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp quasimodo_msgs soma_llsd_msgs mongodb_store) +find_package(catkin REQUIRED COMPONENTS roscpp quasimodo_msgs soma_llsd_msgs mongodb_store pcl_ros cv_bridge eigen_conversions tf_conversions) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) +# Find PCL +find_package(PCL 1.6 REQUIRED COMPONENTS common io search visualization surface kdtree features surface segmentation octree filter keypoints) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) +find_package(OpenCV REQUIRED) + ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed @@ -131,6 +138,8 @@ add_dependencies(conversions ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORT ## Specify libraries to link a library or executable target against target_link_libraries(conversions + ${OpenCV_LIBS} + ${PCL_LIBRARIES} ${catkin_LIBRARIES} ) diff --git a/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h index ba480705..5353eea8 100644 --- a/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h +++ b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h @@ -8,12 +8,21 @@ #include #include +#include +#include +#include + namespace quasimodo_conversions { void model_to_soma_segment(ros::NodeHandle& n, const quasimodo_msgs::model& model, soma_llsd_msgs::Segment& segment); void soma_segment_to_model(ros::NodeHandle& n, const soma_llsd_msgs::Segment& segment, quasimodo_msgs::model& model); void soma_observation_to_frame(ros::NodeHandle& n, const soma_llsd_msgs::Observation& obs, quasimodo_msgs::rgbd_frame& frame); void frame_to_soma_observation(ros::NodeHandle& n, const quasimodo_msgs::rgbd_frame& frame, soma_llsd_msgs::Observation& obs); +void raw_frames_to_soma_scene(const cv::Mat& rgb, const cv::Mat& depth, + const pcl::PointCloud::ConstPtr& cloud, + const Eigen::Matrix4d& pose, const Eigen::Matrix3d& K, + const std::string& waypoint, const std::string episode_id, + soma_llsd_msgs::Scene& scene); } // namespace quasimodo_conversions diff --git a/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp index 316979af..8a860efd 100644 --- a/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp +++ b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp @@ -2,8 +2,38 @@ #include #include +#include +#include +#include +#include +#include + namespace quasimodo_conversions { +void convert_to_img_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image) +{ + cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage); + cv_pub_ptr->image = cv_image; + cv_pub_ptr->encoding = "bgr8"; + ros_image = *cv_pub_ptr->toImageMsg(); +} + +void convert_to_depth_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image) +{ + cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage); + cv_pub_ptr->image = cv_image; + cv_pub_ptr->encoding = "mono16"; + ros_image = *cv_pub_ptr->toImageMsg(); +} + +void convert_to_mask_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image) +{ + cv_bridge::CvImagePtr cv_pub_ptr(new cv_bridge::CvImage); + cv_pub_ptr->image = cv_image; + cv_pub_ptr->encoding = "mono8"; + ros_image = *cv_pub_ptr->toImageMsg(); +} + void model_to_soma_segment(ros::NodeHandle& n, const quasimodo_msgs::model& model, soma_llsd_msgs::Segment& segment) { ros::ServiceClient client = n.serviceClient("/soma_llsd/insert_scene"); @@ -133,4 +163,53 @@ void soma_observation_to_frame(ros::NodeHandle& n, const soma_llsd_msgs::Observa frame.pose = obs.pose; // not good actually but whatevs } +//string episode_id +//string waypoint +//string meta_data +//uint32 timestamp + +//tf/tfMessage transform +//sensor_msgs/PointCloud2 cloud +//sensor_msgs/Image rgb_img +//sensor_msgs/Image depth_img +//sensor_msgs/CameraInfo camera_info +//geometry_msgs/Pose robot_pose +//--- +//bool result +//soma_llsd_msgs/Scene response + +// ### response has id field! ### + +void raw_frames_to_soma_scene(const cv::Mat& rgb, const cv::Mat& depth, + const pcl::PointCloud::ConstPtr& cloud, + const Eigen::Matrix4d& pose, const Eigen::Matrix3d& K, + const std::string& waypoint, const std::string episode_id, + soma_llsd_msgs::Scene& scene) +{ + convert_to_img_msg(rgb, scene.rgb_img); + convert_to_depth_msg(depth, scene.depth_img); + pcl::toROSMsg(*cloud, scene.cloud); + tf::poseEigenToMsg(Eigen::Affine3d(pose), scene.robot_pose); + sensor_msgs::CameraInfo info; + info.K.at(0) = K(0, 0); + info.K.at(1) = K(0, 1); + info.K.at(2) = K(0, 2); + info.K.at(3) = K(1, 0); + info.K.at(4) = K(1, 1); + info.K.at(5) = K(1, 2); + info.K.at(8) = K(2, 2); + + info.P.at(0) = K(0, 0); + info.P.at(1) = K(0, 1); + info.P.at(2) = K(0, 2); + info.P.at(4) = K(1, 0); + info.P.at(5) = K(1, 1); + info.P.at(6) = K(1, 2); + info.P.at(10) = K(2, 2); + info.height = rgb.rows; + info.width = rgb.cols; + scene.waypoint = waypoint; + scene.episode_id = episode_id; +} + } // namespace quasimodo_conversions From 24f70ca1825964ee312b6d8d19b99ed2ebb3f3f2 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 21 Nov 2016 14:04:59 +0100 Subject: [PATCH 215/255] integrate people rejection --- quasimodo/quasimodo_brain/CMakeLists.txt | 2 +- quasimodo/quasimodo_brain/package.xml | 4 + .../metaroom_additional_view_processing.cpp | 272 ++++++++++-------- .../quasimodo_models/include/core/Util.h | 2 +- .../src/modelupdater/ModelUpdater.cpp | 2 +- .../DistanceWeightFunction2PPR2.cpp | 10 +- 6 files changed, 168 insertions(+), 124 deletions(-) diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index d41c5962..0125fb78 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(quasimodo_brain) -find_package(catkin REQUIRED COMPONENTS roscpp soma_msgs object_manager object_manager_msgs semantic_map_msgs soma_manager quasimodo_msgs quasimodo_conversions quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge) +find_package(catkin REQUIRED COMPONENTS roscpp soma_msgs object_manager object_manager_msgs semantic_map_msgs soma_manager local_metric_map_people_rejection quasimodo_msgs quasimodo_conversions quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge) set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fPIC -std=c++0x -o popcnt -mssse3") diff --git a/quasimodo/quasimodo_brain/package.xml b/quasimodo/quasimodo_brain/package.xml index 320ebc1a..6d689239 100644 --- a/quasimodo/quasimodo_brain/package.xml +++ b/quasimodo/quasimodo_brain/package.xml @@ -59,6 +59,10 @@ semantic_map_msgs object_manager_msgs object_manager + + local_metric_map_people_rejection + local_metric_map_people_rejection + object_manager object_manager_msgs semantic_map_msgs diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index c7f7f284..4de4a1a5 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -99,6 +99,9 @@ #include #include +#include +#include +#include std::vector< ros::ServiceServer > m_DynamicObjectsServiceServers; std::vector< ros::ServiceServer > m_GetDynamicObjectServiceServers; @@ -641,7 +644,7 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); } -//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); + //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( sweep, reg); mu->occlusion_penalty = 15; @@ -649,13 +652,13 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri mu->viewer = viewer; sweep->recomputeModelPoints(); -//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); + //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); reglib::Model * fullmodel = new reglib::Model(); fullmodel->savePath = savePath+"/"; fullmodel->frames = sweep->frames; fullmodel->relativeposes = sweep->relativeposes; fullmodel->modelmasks = sweep->modelmasks; -//printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); + //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); if(frames.size() > 0){ reglib::RegistrationRefinement * refinement = new reglib::RegistrationRefinement(); @@ -706,7 +709,7 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri bgmassreg->stopval = 0.0005; bgmassreg->addModel(sweep); bgmassreg->setData(frames,masks); -// exit(0); + // exit(0); //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); @@ -879,13 +882,16 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr if ( ! boost::filesystem::exists( path ) ){return 0;} + + + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); store_old_xml = objectFiles.size() == 0; SimpleXMLParser parser; SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path,std::vector()); -// if(current_roomData.roomWaypointId.compare("ReceptionDesk") != 0){return 0;} + // if(current_roomData.roomWaypointId.compare("ReceptionDesk") != 0){return 0;} current_roomData = parser.loadRoomFromXML(path,std::vector{"RoomIntermediateCloud","IntermediatePosition"}); if(current_roomData.vIntermediateRoomClouds.size() != 17){return 0;} @@ -965,14 +971,36 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr printf("no previous...\n"); } + if(bgs.size() > 0){ + + std::vector peopleMasks; + for(unsigned int i = 0; i < models.front()->frames.size(); i++){ + cv::Mat peoplemask; + int width = models.front()->frames[i]->camera->width; + int height = models.front()->frames[i]->camera->height; + peoplemask.create(height,width,CV_8UC1); + unsigned int nr_pixels = width*height; + for(unsigned int j = 0; j < nr_pixels; j++){peoplemask.data[j] = 0;} + peopleMasks.push_back(peoplemask); + } + + std::vector > > detections; + detections = metaroom_detections::detections_for_xml(path, "intermediate_deep_detection"); + + unsigned int count = 0; + for (std::vector >& image_dets : detections) { + for (std::tuple& det : image_dets) { + cv::rectangle(peopleMasks[count], cv::Rect(std::get<0>(det),std::get<1>(det),std::get<2>(det),std::get<3>(det)), cv::Scalar(255,255,255), -1); + } + count++; + } + auto sweep = SimpleXMLParser::loadRoomFromXML(path, std::vector{},false); - printf("models.front()->frames.size() = %i\n",models.front()->frames.size()); quasimodo_brain::segment(bgs,models,internal,external,dynamic,visualization_lvl,saveVisuals); remove_old_seg(sweep_folder); - if(models.size() == 0){ returnval = 2;} else{ returnval = 3;} @@ -1000,6 +1028,7 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr CloudPtr cloud_cluster (new Cloud()); Eigen::Matrix4d first = model->frames.front()->pose; + int peopleoverlaps = 0; for(unsigned int j = 0; j < mod_fr.size(); j++){ reglib::RGBDFrame * frame = mod_fr[j]; @@ -1027,10 +1056,12 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr const unsigned int width = camera->width; const unsigned int height = camera->height; + cv::Mat peoplemask = peopleMasks[j]; + cv::Mat mask; mask.create(height,width,CV_8UC1); unsigned char * maskdata = (unsigned char *)(mask.data); - + unsigned char * peoplemaskdata = (unsigned char *)(peoplemask.data); bool containsData = false; for(unsigned int w = 0; w < width;w++){ @@ -1058,6 +1089,7 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr sumy += p.y; sumz += p.z; sum ++; + if(peoplemaskdata[ind] != 0){peopleoverlaps++;} } }else{ maskdata[ind] = 0; @@ -1075,121 +1107,125 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr cloud_cluster->height = 1; cloud_cluster->is_dense = true; - if(masks.size() > 0){ - - - if(store_old_xml){ - std::stringstream ss; - ss << "object_"; - ss << (dynamicCounter-1); - - // create and save dynamic object - DynamicObject::Ptr roomObject(new DynamicObject()); - roomObject->setCloud(cloud_cluster); - roomObject->setTime(sweep.roomLogStartTime); - roomObject->m_roomLogString = sweep.roomLogName; - roomObject->m_roomStringId = sweep.roomWaypointId; - roomObject->m_roomRunNumber = sweep.roomRunNumber; - // // create label from room log time; could be useful later on, and would resolve ambiguities - std::stringstream ss_obj; - ss_obj<m_label = tmp; - roomObject->setLabel(tmp); - std::string xml_file = objectparser.saveAsXML(roomObject); - printf("xml_file: %s\n",xml_file.c_str()); - } - - char buf [1024]; - sprintf(buf,"%s/dynamic_obj%10.10i.pcd",sweep_folder.c_str(),dynamicCounter-1); - pcl::io::savePCDFileBinaryCompressed(std::string(buf),*cloud_cluster); - - *dyncloud += *cloud_cluster; - - - // std::string objectpcd = std::string(buf);//object.substr(0,object.size()-4); - // std::cout << objectpcd.substr(0,objectpcd.size()-4) << std::endl; - - sprintf(buf,"%s/dynamic_obj%10.10i.xml",sweep_folder.c_str(),dynamicCounter-1); - printf("saving dynamic objec: %s\n",buf); - QFile file(buf); - if (file.exists()){file.remove();} - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("Object"); - xmlWriter->writeAttribute("object_number", QString::number(dynamicCounter-1)); - xmlWriter->writeAttribute("classname", QString("")); - xmlWriter->writeAttribute("instancename", QString("")); - xmlWriter->writeAttribute("tags", QString("")); - xmlWriter->writeStartElement("Mean"); - xmlWriter->writeAttribute("x", QString::number(sumx/sum)); - xmlWriter->writeAttribute("y", QString::number(sumy/sum)); - xmlWriter->writeAttribute("z", QString::number(sumz/sum)); - xmlWriter->writeEndElement(); + printf("peopleoverlaps: %i\n",peopleoverlaps); + if(masks.size() > 0){ + if(peopleoverlaps == 0){ + + if(store_old_xml){ + std::stringstream ss; + ss << "object_"; + ss << (dynamicCounter-1); + + // create and save dynamic object + DynamicObject::Ptr roomObject(new DynamicObject()); + roomObject->setCloud(cloud_cluster); + roomObject->setTime(sweep.roomLogStartTime); + roomObject->m_roomLogString = sweep.roomLogName; + roomObject->m_roomStringId = sweep.roomWaypointId; + roomObject->m_roomRunNumber = sweep.roomRunNumber; + // // create label from room log time; could be useful later on, and would resolve ambiguities + std::stringstream ss_obj; + ss_obj<m_label = tmp; + roomObject->setLabel(tmp); + std::string xml_file = objectparser.saveAsXML(roomObject); + printf("xml_file: %s\n",xml_file.c_str()); + } - for(unsigned int j = 0; j < masks.size(); j++){ char buf [1024]; - sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),dynamicCounter-1,imgnr[j]); - cv::imwrite(buf, masks[j] ); + sprintf(buf,"%s/dynamic_obj%10.10i.pcd",sweep_folder.c_str(),dynamicCounter-1); + pcl::io::savePCDFileBinaryCompressed(std::string(buf),*cloud_cluster); + + *dyncloud += *cloud_cluster; + + + // std::string objectpcd = std::string(buf);//object.substr(0,object.size()-4); + // std::cout << objectpcd.substr(0,objectpcd.size()-4) << std::endl; + + sprintf(buf,"%s/dynamic_obj%10.10i.xml",sweep_folder.c_str(),dynamicCounter-1); + printf("saving dynamic objec: %s\n",buf); + QFile file(buf); + if (file.exists()){file.remove();} + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){std::cerr<<"Could not open file "<< buf <<" to save dynamic object as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Object"); + xmlWriter->writeAttribute("object_number", QString::number(dynamicCounter-1)); + xmlWriter->writeAttribute("classname", QString("")); + xmlWriter->writeAttribute("instancename", QString("")); + xmlWriter->writeAttribute("tags", QString("")); + xmlWriter->writeStartElement("Mean"); + xmlWriter->writeAttribute("x", QString::number(sumx/sum)); + xmlWriter->writeAttribute("y", QString::number(sumy/sum)); + xmlWriter->writeAttribute("z", QString::number(sumz/sum)); + xmlWriter->writeEndElement(); - int width = masks[j].cols; - int height = masks[j].rows; - - int maxw = 0; - int minw = width; - int maxh = 0; - int minh = height; - for(int w = 0; w < width; w++){ - for(int h = 0; h < height; h++){ - if(masks[j].data[h*width+w] != 0){ - maxw = std::max(maxw,w); - minw = std::min(minw,w); - maxh = std::max(maxh,h); - minh = std::min(minh,h); + + for(unsigned int j = 0; j < masks.size(); j++){ + char buf [1024]; + sprintf(buf,"%s/dynamicmask_%i_%i.png",sweep_folder.c_str(),dynamicCounter-1,imgnr[j]); + cv::imwrite(buf, masks[j] ); + + int width = masks[j].cols; + int height = masks[j].rows; + + int maxw = 0; + int minw = width; + int maxh = 0; + int minh = height; + for(int w = 0; w < width; w++){ + for(int h = 0; h < height; h++){ + if(masks[j].data[h*width+w] != 0){ + maxw = std::max(maxw,w); + minw = std::min(minw,w); + maxh = std::max(maxh,h); + minh = std::min(minh,h); + } } } - } - double ratio = 0.15; - int diffw = maxw-minw; - int diffh = maxh-minh; + double ratio = 0.15; + int diffw = maxw-minw; + int diffh = maxh-minh; - maxw = std::min(width-1 ,int(maxw+ratio*diffw)); - minw = std::max(0 ,int(minw-ratio*diffw)); - maxh = std::min(height-1,int(maxh+ratio*diffh)); - minh = std::max(0 ,int(minh-ratio*diffh)); + maxw = std::min(width-1 ,int(maxw+ratio*diffw)); + minw = std::max(0 ,int(minw-ratio*diffw)); + maxh = std::min(height-1,int(maxh+ratio*diffh)); + minh = std::max(0 ,int(minh-ratio*diffh)); - diffw = maxw-minw; - diffh = maxh-minh; + diffw = maxw-minw; + diffh = maxh-minh; - cv::Mat image = mod_fr[imgnr[j]]->rgb.clone(); - cv::Rect myROI(minw, minh, diffw, diffh); - cv::Mat localimg = image(myROI); + cv::Mat image = mod_fr[imgnr[j]]->rgb.clone(); + cv::Rect myROI(minw, minh, diffw, diffh); + cv::Mat localimg = image(myROI); - char buf2 [1024]; - sprintf(buf2,"/home/johane/imgregion/region%10.10i.png",totalcounter++); - cv::imwrite(buf2, localimg ); + char buf2 [1024]; + sprintf(buf2,"/home/johane/imgregion/region%10.10i.png",totalcounter++); + cv::imwrite(buf2, localimg ); - printf("saving dynamic mask: dynamicmask_%i_%i.png\n",dynamicCounter-1,imgnr[j]); + printf("saving dynamic mask: dynamicmask_%i_%i.png\n",dynamicCounter-1,imgnr[j]); + + sprintf(buf,"dynamicmask_%i_%i.png",dynamicCounter-1,imgnr[j]); + xmlWriter->writeStartElement("Mask"); + xmlWriter->writeAttribute("filename", QString(buf)); + xmlWriter->writeAttribute("image_number", QString::number(imgnr[j])); + xmlWriter->writeEndElement(); + } - sprintf(buf,"dynamicmask_%i_%i.png",dynamicCounter-1,imgnr[j]); - xmlWriter->writeStartElement("Mask"); - xmlWriter->writeAttribute("filename", QString(buf)); - xmlWriter->writeAttribute("image_number", QString::number(imgnr[j])); xmlWriter->writeEndElement(); + xmlWriter->writeEndDocument(); + delete xmlWriter; } - - xmlWriter->writeEndElement(); - xmlWriter->writeEndDocument(); - delete xmlWriter; dynamicCounter++; + }else{break;} } @@ -1360,7 +1396,7 @@ void trainMetaroom(std::string path){ bgmassreg->stopval = 0.0005; bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); -exit(0); + exit(0); savePoses(overall_folder+"/"+posepath,bgmfr.poses,17); fullmodel->fullDelete(); delete fullmodel; @@ -1899,12 +1935,12 @@ void processSweepForDatabase(std::string path, std::string savePath){ sweep->frames[i]->pose *= change; } -// string meta_data -// uint32 timestamp -// tf/tfMessage transform -// --- -// bool result -// soma_llsd_msgs/Scene response + // string meta_data + // uint32 timestamp + // tf/tfMessage transform + // --- + // bool result + // soma_llsd_msgs/Scene response ros::ServiceClient client = np->serviceClient("/soma_llsd/insert_scene"); ROS_INFO("Waiting for /soma_llsd/insert_scene service..."); @@ -1952,10 +1988,10 @@ void processSweepForDatabase(std::string path, std::string savePath){ scene.request.waypoint = current_waypointid; scene.request.episode_id = roomData.roomRunNumber; -// if (!client.call(scene)) { -// ROS_ERROR("Failed to call service /soma_llsd/insert_scene"); -// return; -// } + // if (!client.call(scene)) { + // ROS_ERROR("Failed to call service /soma_llsd/insert_scene"); + // return; + // } cv_bridge::CvImage maskBridgeImage; @@ -1968,7 +2004,7 @@ void processSweepForDatabase(std::string path, std::string savePath){ obs.image_mask = *(maskBridgeImage.toImageMsg()); obs.pose = segpose; obs.id = "frame"+std::to_string(frame->id); -// obs.timestamp = ros::Time::now().nsec; + // obs.timestamp = ros::Time::now().nsec; sweepsegment.observations.push_back(obs); } diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index 1de3fe18..051c91f5 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -75,7 +75,7 @@ std::vector partition_graph(std::vector< std::vector< float > > & scores); inline double getNoise(double depth){return depth*depth;} inline double getInformation(double depth){ - if(depth == 0 || depth > 4){return 0.00000000001;} + //if(depth == 0 || depth > 4){return 0.00000000001;} double n = getNoise(depth); return 1.0/(n*n); } diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index cb180bd6..c02ef199 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2447,7 +2447,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dfunc = dfuncTMP; dfuncTMP->startreg = 0.00; dfuncTMP->max_under_mean = false; - dfuncTMP->debugg_print = false; + dfuncTMP->debugg_print = true; dfuncTMP->bidir = true; dfuncTMP->zeromean = false; dfuncTMP->maxp = 0.9999; diff --git a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp index bd82fee4..d52477d5 100644 --- a/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp +++ b/quasimodo/quasimodo_models/src/weightfunctions/DistanceWeightFunction2PPR2.cpp @@ -392,8 +392,11 @@ double scoreCurrent3(double mul, double mean, double stddiv, double power, std:: double inp = -0.5*pow(dx,power); if(inp < cutoff_exp){sum += Y[i];} else{ - double diff = Y[i]*Y[i]*(mul*exp(inp) - Y[i]); - if(diff > 0){ sum += costpen*diff;} + //double diff = Y[i]*Y[i]*(mul*exp(inp) - Y[i]); + double diff = (mul*exp(inp) - Y[i]); + // if(diff > 0){ sum += costpen*diff;} + // else{ sum -= diff;} + if(diff > 0){ sum += diff*diff;} else{ sum -= diff;} } } @@ -459,7 +462,8 @@ double scoreCurrent2(double bias, double mul, double mean, double stddiv, std::v if(inp < cutoff_exp){sum += Y[i];} else{ double diff = mul*exp(info*dx*dx) - Y[i]; - if(diff > 0){ sum += costpen*diff;} + //if(diff > 0){ sum += costpen*diff;} + if(diff > 0){ sum += diff*diff;} else{ sum -= diff;} } } From a0dacc77545cc509d4aa1d17963e3e4154d2ea79 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 21 Nov 2016 14:06:14 +0100 Subject: [PATCH 216/255] cleaning xml parser --- .../include/semantic_map/room_xml_parser.hpp | 67 +++++++------------ 1 file changed, 23 insertions(+), 44 deletions(-) diff --git a/semantic_map/include/semantic_map/room_xml_parser.hpp b/semantic_map/include/semantic_map/room_xml_parser.hpp index aa0483b1..bc198d3d 100644 --- a/semantic_map/include/semantic_map/room_xml_parser.hpp +++ b/semantic_map/include/semantic_map/room_xml_parser.hpp @@ -55,14 +55,13 @@ bool SemanticRoomXMLParser::setRootFolderFromRoomXml(std::string room template std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoom& aRoom, std::string xmlFile, bool verbose ) { -//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} + printf("saveRoomAsXML\n"); // create folder structure QString roomLogName(aRoom.getRoomLogName().c_str()); int index = roomLogName.indexOf('_'); QString date = roomLogName.left(index); QString patrol = roomLogName.right(roomLogName.size()-index-1); -//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} QString dateFolder = QString(m_RootFolder)+date; if (!QDir(dateFolder).exists()) { @@ -75,7 +74,6 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoom::saveRoomAsXML(SemanticRoom::saveRoomAsXML(SemanticRoomsetDevice(&file); @@ -123,7 +119,6 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteCharacters(aRoom.getRoomStringId().c_str()); xmlWriter->writeEndElement(); -//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} // RoomLogStartTime xmlWriter->writeStartElement("RoomLogStartTime"); boost::posix_time::ptime roomLogStartTime = aRoom.getRoomLogStartTime(); @@ -141,31 +136,15 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteAttribute("filename",cloudFilename); - xmlWriter->writeEndElement(); -//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} + xmlWriter->writeEndElement(); if (aRoom.getCompleteRoomCloudLoaded()) // only save the cloud file if it's been loaded - { - //if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} + { QFile file(completeCloudFilename); // if (!file.exists()) - { - //if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} + { if (aRoom.getCompleteRoomCloud()->points.size()>0) - { - - //if(verbose){printf("aRoom.getCompleteRoomCloud()->points.size(): %i\n",aRoom.getCompleteRoomCloud()->points.size());} - //if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} - std::string fullcloudpath = completeCloudFilename.toStdString(); - if(verbose){printf("fullcloudpath: %s\n",fullcloudpath.c_str());} - - //CloudPtr fullcld = aRoom.getCompleteRoomCloud(); - - Cloud fullcld = *(aRoom.getCompleteRoomCloud()); - - //pcl::io::savePCDFileBinary(completeCloudFilename.toStdString(), *aRoom.getCompleteRoomCloud()); - //pcl::io::savePCDFileBinary(fullcloudpath, *fullcld); - pcl::io::savePCDFileBinaryCompressed(fullcloudpath, fullcld); - //if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} + { + pcl::io::savePCDFileBinary(completeCloudFilename.toStdString(), *aRoom.getCompleteRoomCloud()); if (verbose) { ROS_INFO_STREAM("Saving complete cloud file name "<::saveRoomAsXML(SemanticRoomwriteStartElement("RoomInteriorCloud"); QString interiorCloudFilenameLocal("interior_cloud.pcd"); @@ -187,7 +166,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoompoints.size()>0) { - pcl::io::savePCDFileBinary(interiorCloudFilename.toStdString(), *aRoom.getInteriorRoomCloud()); + pcl::io::savePCDFileBinary(interiorCloudFilename.toStdString(), *aRoom.getInteriorRoomCloud()); if (verbose) { ROS_INFO_STREAM("Saving interior cloud file name "<::saveRoomAsXML(SemanticRoomwriteStartElement("RoomDeNoisedCloud"); QString denoisedCloudFilenameLocal("denoised_cloud.pcd"); @@ -210,7 +189,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoompoints.size()) { - pcl::io::savePCDFileBinary(denoisedCloudFilename.toStdString(), *aRoom.getDeNoisedRoomCloud()); + pcl::io::savePCDFileBinary(denoisedCloudFilename.toStdString(), *aRoom.getDeNoisedRoomCloud()); if (verbose) { ROS_INFO_STREAM("Saving denoised cloud file name "<::saveRoomAsXML(SemanticRoom::saveRoomAsXML(SemanticRoom::saveRoomAsXML(SemanticRoomwriteEndElement(); } } -//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} + // RoomCentroid @@ -261,7 +240,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteCharacters(centroidS); xmlWriter->writeEndElement(); -//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} + // Room Transform xmlWriter->writeStartElement("RoomTransform"); Eigen::Matrix4f transform = aRoom.getRoomTransform(); @@ -272,7 +251,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteCharacters(transformS); xmlWriter->writeEndElement(); -//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} + // RoomIntermediateClouds xmlWriter->writeStartElement("RoomIntermediateClouds"); xmlWriter->writeAttribute("pan_start",QString::number(aRoom.m_SweepParameters.m_pan_start)); @@ -298,7 +277,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteAttribute("filename",intermediateCloudLocalPath); if(roomIntermediateCloudsLoaded[i] && aRoom.getSaveIntermediateClouds()) @@ -306,14 +285,14 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteStartElement("RoomIntermediateCloudTransform"); @@ -367,7 +346,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteEndElement(); // RoomIntermediateCloudTransform -//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} + // RoomIntermediateCloudRegisteredTransform std::vector roomIntermediateCloudTransformsRegistered = aRoom.getIntermediateCloudTransformsRegistered(); // TODO: this is not the case as I might be registering only the lower sweep. @@ -390,7 +369,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteAttribute("width",QString::number(camInfo.width)); @@ -417,7 +396,7 @@ std::string SemanticRoomXMLParser::saveRoomAsXML(SemanticRoomwriteAttribute("D",DString); // ROS_INFO_STREAM("D matrix "<::saveRoomAsXML(SemanticRoomwriteEndElement(); // RoomIntermediateClouds -//if(verbose){printf("saveRoomAsXML: %i\n",__LINE__);} + // Intermediate cloud images saveIntermediateImagesToXML(aRoom,xmlWriter, roomFolder.toStdString()); From 9b953c42ae11c68f69d892eeb9fbce6a48d46f70 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 21 Nov 2016 18:44:43 +0100 Subject: [PATCH 217/255] Added new function for adding segments --- .../quasimodo_conversions/conversions.h | 4 +- .../src/quasimodo_conversions/conversions.cpp | 63 ++++++++++++++++--- 2 files changed, 59 insertions(+), 8 deletions(-) diff --git a/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h index 5353eea8..28a04a89 100644 --- a/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h +++ b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h @@ -18,11 +18,13 @@ void model_to_soma_segment(ros::NodeHandle& n, const quasimodo_msgs::model& mode void soma_segment_to_model(ros::NodeHandle& n, const soma_llsd_msgs::Segment& segment, quasimodo_msgs::model& model); void soma_observation_to_frame(ros::NodeHandle& n, const soma_llsd_msgs::Observation& obs, quasimodo_msgs::rgbd_frame& frame); void frame_to_soma_observation(ros::NodeHandle& n, const quasimodo_msgs::rgbd_frame& frame, soma_llsd_msgs::Observation& obs); -void raw_frames_to_soma_scene(const cv::Mat& rgb, const cv::Mat& depth, +void raw_frames_to_soma_scene(ros::NodeHandle& n, const cv::Mat& rgb, const cv::Mat& depth, const pcl::PointCloud::ConstPtr& cloud, const Eigen::Matrix4d& pose, const Eigen::Matrix3d& K, const std::string& waypoint, const std::string episode_id, soma_llsd_msgs::Scene& scene); +void add_masks_to_soma_segment(ros::NodeHandle& n, std::vector& scene_ids, std::vector& masks, + std::vector >& poses, soma_llsd_msgs::Segment& segment); } // namespace quasimodo_conversions diff --git a/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp index 8a860efd..0816ef42 100644 --- a/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp +++ b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp @@ -1,6 +1,8 @@ #include #include #include +#include +#include #include #include @@ -180,16 +182,26 @@ void soma_observation_to_frame(ros::NodeHandle& n, const soma_llsd_msgs::Observa // ### response has id field! ### -void raw_frames_to_soma_scene(const cv::Mat& rgb, const cv::Mat& depth, +void raw_frames_to_soma_scene(ros::NodeHandle& n, const cv::Mat& rgb, const cv::Mat& depth, const pcl::PointCloud::ConstPtr& cloud, const Eigen::Matrix4d& pose, const Eigen::Matrix3d& K, const std::string& waypoint, const std::string episode_id, soma_llsd_msgs::Scene& scene) { - convert_to_img_msg(rgb, scene.rgb_img); - convert_to_depth_msg(depth, scene.depth_img); - pcl::toROSMsg(*cloud, scene.cloud); - tf::poseEigenToMsg(Eigen::Affine3d(pose), scene.robot_pose); + ros::ServiceClient client = n.serviceClient("/soma_llsd/insert_scene"); + ROS_INFO("Waiting for /soma_llsd/insert_scene service..."); + if (!client.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/insert_scene service!"); + return; + } + ROS_INFO("Got /soma_llsd/insert_scene service"); + + soma_llsd::InsertScene scene_srv; + + convert_to_img_msg(rgb, scene_srv.request.rgb_img); + convert_to_depth_msg(depth, scene_srv.request.depth_img); + pcl::toROSMsg(*cloud, scene_srv.request.cloud); + tf::poseEigenToMsg(Eigen::Affine3d(pose), scene_srv.request.robot_pose); sensor_msgs::CameraInfo info; info.K.at(0) = K(0, 0); info.K.at(1) = K(0, 1); @@ -208,8 +220,45 @@ void raw_frames_to_soma_scene(const cv::Mat& rgb, const cv::Mat& depth, info.P.at(10) = K(2, 2); info.height = rgb.rows; info.width = rgb.cols; - scene.waypoint = waypoint; - scene.episode_id = episode_id; + scene_srv.request.camera_info = info; + scene_srv.request.waypoint = waypoint; + scene_srv.request.episode_id = episode_id; + + if (!client.call(scene_srv) || scene_srv.response.result) { + ROS_ERROR("Failed to call service /soma_llsd/insert_scene"); + return; + } + + scene = scene_srv.response.response; +} + +void add_masks_to_soma_segment(ros::NodeHandle& n, std::vector& scene_ids, std::vector& masks, + std::vector >& poses, soma_llsd_msgs::Segment& segment) +{ + ros::ServiceClient client = n.serviceClient("/soma_llsd/insert_segment"); + ROS_INFO("Waiting for /soma_llsd/insert_segment service..."); + if (!client.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/insert_segment service!"); + return; + } + ROS_INFO("Got /soma_llsd/insert_segment service"); + + soma_llsd::InsertSegment segment_srv; + + size_t N = masks.size(); + segment_srv.request.observations.resize(N); + for (size_t i = 0; i < N; ++i) { + convert_to_mask_msg(masks[i], segment_srv.request.observations[i].image_mask); + tf::poseEigenToMsg(Eigen::Affine3d(poses[i]), segment_srv.request.observations[i].pose); + segment_srv.request.observations[i].scene_id = scene_ids[i]; + } + + if (!client.call(segment_srv) || segment_srv.response.result) { + ROS_ERROR("Failed to call service /soma_llsd/insert_segment"); + return; + } + + segment = segment_srv.response.response; } } // namespace quasimodo_conversions From d5bd83ca192385fbe5f9c9a19fbaa7d772496f9d Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 21 Nov 2016 18:54:30 +0100 Subject: [PATCH 218/255] working on soma integration --- .../metaroom_additional_view_processing.cpp | 322 +++++++++++++++++- .../quasimodo_conversions/conversions.h | 5 + 2 files changed, 326 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 4de4a1a5..549eaaec 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -141,6 +141,7 @@ ros::Publisher model_pub; std::vector< ros::Publisher > m_PublisherStatuss; std::vector< ros::Publisher > out_pubs; std::vector< ros::Publisher > model_pubs; +std::vector< ros::Publisher > soma_segment_id_pubs; ros::Publisher roomObservationCallback_pubs; std::string saveVisuals = ""; @@ -1660,6 +1661,314 @@ bool testDynamicObjectServiceCallback(std::string path){ } + +cv::Mat getMaskImg(cv::Mat image, cv::Mat mask){ + unsigned int width = image.rows; + unsigned int height = image.cols; + int maxw = 0; + int minw = width; + int maxh = 0; + int minh = height; + for(int w = 0; w < width; w++){ + for(int h = 0; h < height; h++){ + if(mask.data[h*width+w] != 0){ + maxw = std::max(maxw,w); + minw = std::min(minw,w); + maxh = std::max(maxh,h); + minh = std::min(minh,h); + } + } + } + + double ratio = 0.15; + int diffw = maxw-minw; + int diffh = maxh-minh; + + maxw = std::min(int(width-1 ),int(maxw+ratio*diffw)); + minw = std::max(int(0 ),int(minw-ratio*diffw)); + maxh = std::min(int(height-1),int(maxh+ratio*diffh)); + minh = std::max(int(0 ),int(minh-ratio*diffh)); + + diffw = maxw-minw; + diffh = maxh-minh; + + cv::Rect myROI(minw, minh, diffw, diffh); + cv::Mat localimg = image(myROI); + return localimg; +} + +geometry_msgs::Pose getPoseMSG(Eigen::Matrix4d pose){ + geometry_msgs::Pose pose1; + tf::poseEigenToMsg (Eigen::Affine3d(pose), pose1); + return pose1; +} + +void sendRoomToSomaLLSD(std::string path){ + printf("sendRoomToSomaLLSD(%s)\n",path.c_str()); + + + if ( ! boost::filesystem::exists( path ) ){return;} + + + SimpleXMLParser parser; + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(path,std::vector(),false,false); + std::string waypoint = roomData.roomWaypointId; + std::string episode_id = roomData.roomLogName; + + + + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + + + QFile file((sweep_folder+"ViewGroup.xml").c_str()); + if (!file.exists()){ + ROS_ERROR("Could not open file %s to load room.",(sweep_folder+"ViewGroup.xml").c_str()); + return; + } + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<<(sweep_folder+"ViewGroup.xml").c_str()); + + std::vector< soma_llsd_msgs::Scene > scenes; + std::vector< Eigen::Matrix4d > regposes; + std::vector< cv::Mat > rgbs; + std::vector< cv::Mat > depths; + + + + QXmlStreamReader* xmlReader = new QXmlStreamReader(&file); + while (!xmlReader->atEnd() && !xmlReader->hasError()) + { + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()) + { + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + return; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement) + { + + if (xmlReader->name() == "View") + { + cv::Mat rgb; + cv::Mat depth; + //printf("elementName: %s\n",elementName.toStdString().c_str()); + QXmlStreamAttributes attributes = xmlReader->attributes(); + if (attributes.hasAttribute("RGB")) + { + std::string imgpath = attributes.value("RGB").toString().toStdString(); + printf("rgb filename: %s\n",(sweep_folder+"/"+imgpath).c_str()); + rgb = cv::imread(sweep_folder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + if (attributes.hasAttribute("DEPTH")) + { + std::string imgpath = attributes.value("DEPTH").toString().toStdString(); + printf("depth filename: %s\n",(sweep_folder+"/"+imgpath).c_str()); + depth = cv::imread(sweep_folder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + token = xmlReader->readNext();//Stamp + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//sec + elementName = xmlReader->name().toString(); + int sec = atoi(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//nsec + elementName = xmlReader->name().toString(); + int nsec = atoi(xmlReader->readElementText().toStdString().c_str()); + token = xmlReader->readNext();//end stamp + + token = xmlReader->readNext();//Camera + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//fx + float fx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//fy + float fy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cx + float cx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cy + float cy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Camera + elementName = xmlReader->name().toString(); + + double time = double(sec)+double(nsec)/double(1e9); + + token = xmlReader->readNext();//RegisteredPose + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d regpose = getPose(xmlReader); + regposes.push_back(regpose); + + token = xmlReader->readNext();//RegisteredPose + elementName = xmlReader->name().toString(); + + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d pose = getPose(xmlReader); + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + rgbs.push_back(rgb); + depths.push_back(depth); + + + unsigned char * rgbdata = (unsigned char *)rgb.data; + unsigned short * depthdata = (unsigned short *)depth.data; + const unsigned int width = rgb.cols; + const unsigned int height = rgb.rows; + + const double idepth = 0.001/5.0; + const double ifx = 1.0/fx; + const double ify = 1.0/fy; + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->width = width; + cloud->height = height; + cloud->points.resize(width*height); + + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + int ind = h*width+w; + double z = idepth*double(depthdata[ind]); + + pcl::PointXYZRGB p; + p.b = rgbdata[3*ind+0]; + p.g = rgbdata[3*ind+1]; + p.r = rgbdata[3*ind+2]; + if(z > 0){ + p.x = (double(w) - cx) * z * ifx; + p.y = (double(h) - cy) * z * ify; + p.z = z; + }else{ + p.x = NAN; + p.y = NAN; + p.z = NAN; + } + cloud->points[ind] = p; + } + } + + Eigen::Matrix3d K = Eigen::Matrix3d::Zero(); + K(0,0) = fx; + K(1,1) = fy; + K(0,2) = cx; + K(1,2) = cy; + soma_llsd_msgs::Scene scene; + quasimodo_conversions::raw_frames_to_soma_scene(rgb,depth,cloud, pose, K,waypoint, episode_id,scene); + scenes.push_back(scene); + } + } + } + delete xmlReader; + + //Scenes added to database... + + std::ofstream sceneidfile; + sceneidfile.open (sweep_folder+"/sceneids.txt"); + for(unsigned int i = 0; i < scenes.size(); i++){ + sceneidfile << scenes[i].id; + std::cout << "-------------------------------------------------\n" << scenes[i].id << std::endl; + } + sceneidfile.close(); + + + // string id + // string meta_data + // string[] related_scenes + // Observation[] observations + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("dynamic_obj*.xml")); + for (auto objectFile : objectFiles){ + + std::string object = sweep_folder+objectFile.toStdString(); + printf("object: %s\n",object.c_str()); + + QFile objfile(object.c_str()); + + if (!objfile.exists()){ + ROS_ERROR("Could not open file %s to masks.",object.c_str()); + continue; + } + + objfile.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "< scid; + std::vector< cv::Mat > masks; + std::vector< Eigen::Matrix4d > poses; + + + QXmlStreamReader* xmlReader = new QXmlStreamReader(&objfile); + + while (!xmlReader->atEnd() && !xmlReader->hasError()){ + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()){ + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + break; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "Mask"){ + int number = 0; + cv::Mat mask; + QXmlStreamAttributes attributes = xmlReader->attributes(); + if (attributes.hasAttribute("filename")){ + QString maskpath = attributes.value("filename").toString(); + mask = cv::imread(sweep_folder+"/"+(maskpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); + }else{break;} + + + if (attributes.hasAttribute("image_number")){ + QString depthpath = attributes.value("image_number").toString(); + number = atoi(depthpath.toStdString().c_str()); + printf("number: %i\n",number); + }else{break;} + + scid.push_back(scenes[number].id); + poses.push_back(regposes[number]); + masks.push_back(mask); + } + } + } + delete xmlReader; + +// bool result = true; +// soma_llsd_msgs::Segment response; +// if(result){ +// std::string segmentid = respose.segment_id; +// } + + } + +// for(unsigned int i = 0; i < frames.size(); i++){delete frames[i];} +// return models; + + +} + void processSweep(std::string path, std::string savePath){ @@ -1692,11 +2001,16 @@ void processSweep(std::string path, std::string savePath){ if(sweep_xmls[i].compare(path) == 0){break;} prevind = i; } + if(prevind >= 0){//Submit last metaroom results + //Add previous metaroom to soma llsd + sendRoomToSomaLLSD(sweep_xmls[prevind]); sendMetaroomToServer(sweep_xmls[prevind]); } } + + void roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& obs_msg) { std::cout<<"Room obs message received"<("/quasimodo/segmentation/out/soma_segment_id", 1000)); + } } diff --git a/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h index 5353eea8..ed4cb81b 100644 --- a/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h +++ b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h @@ -14,6 +14,11 @@ namespace quasimodo_conversions { + +void convert_to_img_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image); +void convert_to_depth_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image); +void convert_to_mask_msg(const cv::Mat& cv_image, sensor_msgs::Image& ros_image); + void model_to_soma_segment(ros::NodeHandle& n, const quasimodo_msgs::model& model, soma_llsd_msgs::Segment& segment); void soma_segment_to_model(ros::NodeHandle& n, const soma_llsd_msgs::Segment& segment, quasimodo_msgs::model& model); void soma_observation_to_frame(ros::NodeHandle& n, const soma_llsd_msgs::Observation& obs, quasimodo_msgs::rgbd_frame& frame); From d48b0aade1949581a58143dd8d9d080de4a9a55f Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 21 Nov 2016 20:24:09 +0100 Subject: [PATCH 219/255] soma message integration for segmentation done --- .../metaroom_additional_view_processing.cpp | 25 ++++++++----------- .../quasimodo_conversions/conversions.h | 4 +-- .../src/quasimodo_conversions/conversions.cpp | 18 +++++++------ 3 files changed, 22 insertions(+), 25 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 549eaaec..56d9372a 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1736,7 +1736,7 @@ void sendRoomToSomaLLSD(std::string path){ std::vector< cv::Mat > depths; - + bool allOk = true; QXmlStreamReader* xmlReader = new QXmlStreamReader(&file); while (!xmlReader->atEnd() && !xmlReader->hasError()) { @@ -1873,13 +1873,14 @@ void sendRoomToSomaLLSD(std::string path){ K(0,2) = cx; K(1,2) = cy; soma_llsd_msgs::Scene scene; - quasimodo_conversions::raw_frames_to_soma_scene(rgb,depth,cloud, pose, K,waypoint, episode_id,scene); + allOk = allOk && quasimodo_conversions::raw_frames_to_soma_scene(*np,rgb,depth,cloud, pose, K,waypoint, episode_id,scene); scenes.push_back(scene); } } } delete xmlReader; + if(!allOk){printf("failed to add all scenes to soma, not adding segments\n");return;} //Scenes added to database... std::ofstream sceneidfile; @@ -1890,7 +1891,6 @@ void sendRoomToSomaLLSD(std::string path){ } sceneidfile.close(); - // string id // string meta_data // string[] related_scenes @@ -1913,7 +1913,7 @@ void sendRoomToSomaLLSD(std::string path){ std::vector< std::string > scid; std::vector< cv::Mat > masks; - std::vector< Eigen::Matrix4d > poses; + std::vector< Eigen::Matrix4d, Eigen::aligned_allocator > > poses; QXmlStreamReader* xmlReader = new QXmlStreamReader(&objfile); @@ -1955,18 +1955,13 @@ void sendRoomToSomaLLSD(std::string path){ } delete xmlReader; -// bool result = true; -// soma_llsd_msgs::Segment response; -// if(result){ -// std::string segmentid = respose.segment_id; -// } - + soma_llsd_msgs::Segment segment; + if(quasimodo_conversions::add_masks_to_soma_segment(*np,scid, masks, poses,segment)){ + for(unsigned int i = 0; i < soma_segment_id_pubs.size(); i++){ + //soma_segment_id_pubs[i].publish(segment.id); + } + } } - -// for(unsigned int i = 0; i < frames.size(); i++){delete frames[i];} -// return models; - - } void processSweep(std::string path, std::string savePath){ diff --git a/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h index 59d5d2d0..91b5efb0 100644 --- a/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h +++ b/quasimodo/quasimodo_conversions/include/quasimodo_conversions/conversions.h @@ -23,12 +23,12 @@ void model_to_soma_segment(ros::NodeHandle& n, const quasimodo_msgs::model& mode void soma_segment_to_model(ros::NodeHandle& n, const soma_llsd_msgs::Segment& segment, quasimodo_msgs::model& model); void soma_observation_to_frame(ros::NodeHandle& n, const soma_llsd_msgs::Observation& obs, quasimodo_msgs::rgbd_frame& frame); void frame_to_soma_observation(ros::NodeHandle& n, const quasimodo_msgs::rgbd_frame& frame, soma_llsd_msgs::Observation& obs); -void raw_frames_to_soma_scene(ros::NodeHandle& n, const cv::Mat& rgb, const cv::Mat& depth, +bool raw_frames_to_soma_scene(ros::NodeHandle& n, const cv::Mat& rgb, const cv::Mat& depth, const pcl::PointCloud::ConstPtr& cloud, const Eigen::Matrix4d& pose, const Eigen::Matrix3d& K, const std::string& waypoint, const std::string episode_id, soma_llsd_msgs::Scene& scene); -void add_masks_to_soma_segment(ros::NodeHandle& n, std::vector& scene_ids, std::vector& masks, +bool add_masks_to_soma_segment(ros::NodeHandle& n, std::vector& scene_ids, std::vector& masks, std::vector >& poses, soma_llsd_msgs::Segment& segment); } // namespace quasimodo_conversions diff --git a/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp index 0816ef42..821ea3e7 100644 --- a/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp +++ b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp @@ -182,7 +182,7 @@ void soma_observation_to_frame(ros::NodeHandle& n, const soma_llsd_msgs::Observa // ### response has id field! ### -void raw_frames_to_soma_scene(ros::NodeHandle& n, const cv::Mat& rgb, const cv::Mat& depth, +bool raw_frames_to_soma_scene(ros::NodeHandle& n, const cv::Mat& rgb, const cv::Mat& depth, const pcl::PointCloud::ConstPtr& cloud, const Eigen::Matrix4d& pose, const Eigen::Matrix3d& K, const std::string& waypoint, const std::string episode_id, @@ -192,7 +192,7 @@ void raw_frames_to_soma_scene(ros::NodeHandle& n, const cv::Mat& rgb, const cv:: ROS_INFO("Waiting for /soma_llsd/insert_scene service..."); if (!client.waitForExistence(ros::Duration(1.0))) { ROS_INFO("Failed to get /soma_llsd/insert_scene service!"); - return; + return false; } ROS_INFO("Got /soma_llsd/insert_scene service"); @@ -224,22 +224,23 @@ void raw_frames_to_soma_scene(ros::NodeHandle& n, const cv::Mat& rgb, const cv:: scene_srv.request.waypoint = waypoint; scene_srv.request.episode_id = episode_id; - if (!client.call(scene_srv) || scene_srv.response.result) { + if (!client.call(scene_srv) || !scene_srv.response.result) { ROS_ERROR("Failed to call service /soma_llsd/insert_scene"); - return; + return false; } scene = scene_srv.response.response; + return true; } -void add_masks_to_soma_segment(ros::NodeHandle& n, std::vector& scene_ids, std::vector& masks, +bool add_masks_to_soma_segment(ros::NodeHandle& n, std::vector& scene_ids, std::vector& masks, std::vector >& poses, soma_llsd_msgs::Segment& segment) { ros::ServiceClient client = n.serviceClient("/soma_llsd/insert_segment"); ROS_INFO("Waiting for /soma_llsd/insert_segment service..."); if (!client.waitForExistence(ros::Duration(1.0))) { ROS_INFO("Failed to get /soma_llsd/insert_segment service!"); - return; + return false; } ROS_INFO("Got /soma_llsd/insert_segment service"); @@ -253,12 +254,13 @@ void add_masks_to_soma_segment(ros::NodeHandle& n, std::vector& sce segment_srv.request.observations[i].scene_id = scene_ids[i]; } - if (!client.call(segment_srv) || segment_srv.response.result) { + if (!client.call(segment_srv) || !segment_srv.response.result) { ROS_ERROR("Failed to call service /soma_llsd/insert_segment"); - return; + return false; } segment = segment_srv.response.response; + return true; } } // namespace quasimodo_conversions From 2e7ba09ace4e39acf8cbd5f1167541d8c25f261a Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 22 Nov 2016 11:18:41 +0100 Subject: [PATCH 220/255] Added an option for the starting sweep number --- .../retrieval_processing/launch/processing.launch | 8 +++++--- .../src/retrieval_simulate_observations.cpp | 10 ++++++++-- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/quasimodo/retrieval_processing/launch/processing.launch b/quasimodo/retrieval_processing/launch/processing.launch index 373282b4..6954da56 100644 --- a/quasimodo/retrieval_processing/launch/processing.launch +++ b/quasimodo/retrieval_processing/launch/processing.launch @@ -1,6 +1,7 @@ - - + + + @@ -18,13 +19,14 @@ - + + diff --git a/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp b/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp index 8e6ae444..a8d8e636 100644 --- a/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp +++ b/quasimodo/retrieval_processing/src/retrieval_simulate_observations.cpp @@ -94,6 +94,8 @@ int main(int argc, char** argv) pn.param("data_path", data_path, ""); pn.param("bypass_surfelize", bypass_surfelize, true); + int start_add_map_ind; + pn.param("start_add_map_ind", start_add_map_ind, -1); sweep_xmls = semantic_map_load_utilties::getSweepXmls(data_path, true); if (sweep_xmls.empty()) { @@ -101,10 +103,14 @@ int main(int argc, char** argv) return 0; } - sweep_ind = get_correct_sweep_ind(data_path); // 0; + if (start_add_map_ind < 0) { + sweep_ind = get_correct_sweep_ind(data_path); + } + else { + sweep_ind = start_add_map_ind; + } cout << "Starting at sweep number: " << sweep_ind << endl; - if (bypass_surfelize) { cout << "Advertising " << "/surfelization_done" << endl; string_pub = n.advertise("/surfelization_done", 1); From 88cfc3b14a061fb087ef83942d6958c59e541d74 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 22 Nov 2016 11:21:22 +0100 Subject: [PATCH 221/255] Good parameters for deployment --- .../quasimodo_object_search/launch/object_search.launch | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_object_search/launch/object_search.launch b/quasimodo/quasimodo_object_search/launch/object_search.launch index c6a46182..b2da28b7 100644 --- a/quasimodo/quasimodo_object_search/launch/object_search.launch +++ b/quasimodo/quasimodo_object_search/launch/object_search.launch @@ -1,7 +1,9 @@ - + + + @@ -11,6 +13,8 @@ + + From 5f436ede7e04b90f6301c534f05bcadb323d0ddf Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 22 Nov 2016 18:03:47 +0100 Subject: [PATCH 222/255] Added the visualizer --- .../quasimodo_object_search/CMakeLists.txt | 20 +++-- quasimodo/quasimodo_object_search/package.xml | 5 +- .../scripts/insert_object_server.py | 7 ++ .../src/visualize_database_node.cpp | 80 +++++++++++++++++++ 4 files changed, 105 insertions(+), 7 deletions(-) create mode 100644 quasimodo/quasimodo_object_search/src/visualize_database_node.cpp diff --git a/quasimodo/quasimodo_object_search/CMakeLists.txt b/quasimodo/quasimodo_object_search/CMakeLists.txt index b0343ef2..f7935d57 100644 --- a/quasimodo/quasimodo_object_search/CMakeLists.txt +++ b/quasimodo/quasimodo_object_search/CMakeLists.txt @@ -1,14 +1,21 @@ cmake_minimum_required(VERSION 2.8.3) project(quasimodo_object_search) +add_definitions(-std=c++11) + ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED) +find_package(catkin REQUIRED COMPONENTS quasimodo_msgs pcl_ros mongodb_store) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) +# Find PCL +find_package(PCL 1.6 REQUIRED COMPONENTS common io search visualization surface kdtree features surface segmentation octree filter keypoints) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed @@ -108,7 +115,7 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations -# include_directories(include) +include_directories(${catkin_INCLUDE_DIRS}) ## Declare a C++ library # add_library(quasimodo_object_search2 @@ -121,16 +128,17 @@ catkin_package( # add_dependencies(quasimodo_object_search2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable -# add_executable(quasimodo_object_search2_node src/quasimodo_object_search2_node.cpp) +add_executable(visualize_database_node src/visualize_database_node.cpp) ## Add cmake target dependencies of the executable ## same as for the library above # add_dependencies(quasimodo_object_search2_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -# target_link_libraries(quasimodo_object_search2_node -# ${catkin_LIBRARIES} -# ) +target_link_libraries(visualize_database_node + ${PCL_LIBRARIES} + ${catkin_LIBRARIES} +) ############# ## Install ## diff --git a/quasimodo/quasimodo_object_search/package.xml b/quasimodo/quasimodo_object_search/package.xml index 2ac15879..421cac79 100644 --- a/quasimodo/quasimodo_object_search/package.xml +++ b/quasimodo/quasimodo_object_search/package.xml @@ -40,7 +40,10 @@ catkin - + quasimodo_msgs + mongodb_store + quasimodo_msgs + mongodb_store diff --git a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py index 73f34db0..ca468fde 100755 --- a/quasimodo/quasimodo_object_search/scripts/insert_object_server.py +++ b/quasimodo/quasimodo_object_search/scripts/insert_object_server.py @@ -21,9 +21,12 @@ from quasimodo_msgs.srv import mask_pointclouds, mask_pointcloudsRequest, mask_pointcloudsResponse from quasimodo_msgs.srv import insert_model, insert_modelRequest, insert_modelResponse from geometry_msgs.msg import Pose +from std_msgs.msg import Empty import time import json +pub = () + def insert_model_cb(sreq): msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') @@ -170,11 +173,15 @@ def service_callback(req): resp = False print "Service options are 1: INSERT and 2: REMOVE" + empty_msg = Empty() + pub.publish(empty_msg) + return resp if __name__ == '__main__': rospy.init_node('insert_object_server', anonymous = False) + pub = rospy.Publisher("/model/added_to_db", data_class=Empty, queue_size=None) service = rospy.Service('insert_model_service', insert_model, service_callback) rospy.spin() diff --git a/quasimodo/quasimodo_object_search/src/visualize_database_node.cpp b/quasimodo/quasimodo_object_search/src/visualize_database_node.cpp new file mode 100644 index 00000000..349843ec --- /dev/null +++ b/quasimodo/quasimodo_object_search/src/visualize_database_node.cpp @@ -0,0 +1,80 @@ +#include +#include +#include +#include +#include +#include + +using namespace std; +using PointT = pcl::PointXYZRGB; +using CloudT = pcl::PointCloud; + +class visualization_server { +public: + + ros::NodeHandle n; + mongodb_store::MessageStoreProxy db_client; + ros::Subscriber sub; + boost::shared_ptr viewer; + size_t counter; + + visualization_server() : db_client(n, "quasimodo", "world_state"), viewer(new pcl::visualization::PCLVisualizer("3D Viewer")), counter(0) + { + sub = n.subscribe("/model/added_to_db", 1, &visualization_server::callback, this); + + viewer->setBackgroundColor(1, 1, 1); + + viewer->initCameraParameters(); + + } + + void callback(const std_msgs::Empty& empty_msg) + { + std::vector > messages; + db_client.query(messages); + + viewer->removeAllPointClouds(); + size_t start_counter = counter; + for (boost::shared_ptr& msg : messages) { + if (!msg->removed_at.empty()) { + continue; + } + if (msg->inserted_at.empty()) { + cout << "No insertion for msg with ID: " << msg->object_id << endl; + } + cout << "Adding " << counter << ":th point cloud..." << endl; + CloudT::Ptr cloud(new CloudT); + pcl::fromROSMsg(msg->surfel_cloud, *cloud); + + float offset = 1.0f*float(counter - start_counter); + for (PointT& p : cloud->points) { + p.x += offset; + } + + pcl::visualization::PointCloudColorHandlerRGBField rgb(cloud); + string cloud_name = string("cloud") + std::to_string(counter); + viewer->addPointCloud(cloud, rgb, cloud_name); + viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, cloud_name); + ++counter; + } + } + + void run() + { + while (!viewer->wasStopped()) { + ros::spinOnce(); + viewer->spinOnce(100); + } + } + +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "visualize_database_node"); + + visualization_server vs; + vs.run(); + + return 0; +} From a20c5bd6f5e9a4d13ef6b19e17be7f00db05a0dc Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 22 Nov 2016 23:16:50 +0100 Subject: [PATCH 223/255] lots of changes in process of adding support for the scens objects --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 426 +++++++- quasimodo/quasimodo_brain/src/Util/Util.h | 26 + .../metaroom_additional_view_processing.cpp | 973 ++++++++---------- quasimodo/quasimodo_brain/src/modelserver.cpp | 13 + .../quasimodo_models/include/core/RGBDFrame.h | 1 + .../quasimodo_models/include/core/Util.h | 1 + .../include/core/quasimodo_point.h | 36 + .../quasimodo_models/src/core/Camera.cpp | 5 +- .../quasimodo_models/src/core/RGBDFrame.cpp | 769 ++++++++------ 9 files changed, 1366 insertions(+), 884 deletions(-) create mode 100644 quasimodo/quasimodo_models/include/core/quasimodo_point.h diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index f6c40c70..c1978c8f 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -2,6 +2,424 @@ namespace quasimodo_brain { + +std::vector getSuperPoints(std::string path){ + std::vector spvec; + std::streampos size; + char * memblock; + std::ifstream file (path, ios::in|ios::binary|ios::ate); + if (file.is_open()) { + size = file.tellg(); + if(size == 0){return spvec;} + + memblock = new char [size]; + file.seekg (0, ios::beg); + file.read (memblock, size); + file.close(); + + cout << "the entire file content is in memory"; + + long nr_points = size / (sizeof(float)*12); + printf("loading %i points\n",nr_points); + + float * data = (float *)memblock; + + spvec.resize(nr_points); + long count = 0; + for(unsigned long i = 0; i < nr_points; i++){ + reglib::superpoint & p = spvec[i]; + p.point(0) = data[count++]; + p.point(1) = data[count++]; + p.point(2) = data[count++]; + p.normal(0) = data[count++]; + p.normal(1) = data[count++]; + p.normal(2) = data[count++]; + p.feature(0) = data[count++]; + p.feature(1) = data[count++]; + p.feature(2) = data[count++]; + p.point_information = data[count++]; + p.normal_information = data[count++]; + p.feature_information = data[count++]; + } + + delete[] memblock; + }else{ + std::cout << "Unable to open file superpoint file, go process that data" << std::endl; + } + return spvec; +} + +std::vector getRoomSuperPoints(std::string path, std::string savePath){ + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; + return getSuperPoints(sweep_folder+"/superpoints.bin"); +} + +void transformSuperPoints(std::vector & spvec, Eigen::Matrix4d cp){ + float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); + float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); + float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); + + for(unsigned long i = 0; i < spvec.size(); i++){ + reglib::superpoint & p = spvec[i]; + + float x = p.point(0); + float y = p.point(1); + float z = p.point(2); + float nx = p.normal(0); + float ny = p.normal(1); + float nz = p.normal(2); + + float tx = m00*x + m01*y + m02*z + m03; + float ty = m10*x + m11*y + m12*z + m13; + float tz = m20*x + m21*y + m22*z + m23; + + float tnx = m00*nx + m01*ny + m02*nz; + float tny = m10*nx + m11*ny + m12*nz; + float tnz = m20*nx + m21*ny + m22*nz; + + p.point(0) = tx; + p.point(1) = ty; + p.point(2) = tz; + p.normal(0) = tnx; + p.normal(1) = tny; + p.normal(2) = tnz; + } +} + +void saveSuperPoints(std::string path, std::vector & spvec, Eigen::Matrix4d pose, float ratio_keep){ + printf("saveSuperPoints(%s)\n",path.c_str()); + transformSuperPoints(spvec,pose); + //XYZ RGB NXNYNZ W1 W2 + long sizeofSuperPoint = 3*(3+1); + + std::ofstream file; + file.open(path, ios::out | ios::binary); + if(spvec.size() != 0){ + float * data = new float[spvec.size() * sizeofSuperPoint]; + long added = 0; + long count = 0; + for(unsigned long i = 0; i < spvec.size(); i++){ + if(double(rand() % 1000)*0.001 > ratio_keep ){continue;} + reglib::superpoint & p = spvec[i]; + data[count++] = p.point(0); + data[count++] = p.point(1); + data[count++] = p.point(2); + data[count++] = p.normal(0); + data[count++] = p.normal(1); + data[count++] = p.normal(2); + data[count++] = p.feature(0); + data[count++] = p.feature(1); + data[count++] = p.feature(2); + data[count++] = p.point_information; + data[count++] = p.normal_information; + data[count++] = p.feature_information; + added++; + } + if(added > 0){ + printf("saving %i points\n",added); + file.write( (char*)data, added*sizeofSuperPoint*sizeof(float)); + } + delete[] data; + } + file.close(); +} + +std::vector readPoseXML(std::string xmlFile){ + std::vector poses; + QFile file(xmlFile.c_str()); + + if (!file.exists()){ + ROS_ERROR("Could not open file %s to load poses.",xmlFile.c_str()); + return poses; + } + + file.open(QIODevice::ReadOnly); + ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()){ + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()){ + ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); + return poses; + } + + QString elementName = xmlReader->name().toString(); + + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "Pose"){ + Eigen::Matrix4d pose = quasimodo_brain::getPose(xmlReader); + + token = xmlReader->readNext();//Pose + elementName = xmlReader->name().toString(); + + std::cout << pose << std::endl << std::endl; + poses.push_back(pose); + } + } + } + delete xmlReader; + printf("done readPoseXML\n"); + return poses; +} + +void savePoses(std::string xmlFile, std::vector poses, int maxposes){ + QFile file(xmlFile.c_str()); + if (file.exists()){file.remove();} + + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){ + std::cerr<<"Could not open file "<< xmlFile <<" to save views as XML"<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Poses"); + // xmlWriter->writeAttribute("number_of_poses", QString::number(poses.size())); + for(unsigned int i = 0; i < poses.size() && (maxposes == -1 || int(i) < maxposes); i++){ + printf("saving %i\n",i); + xmlWriter->writeStartElement("Pose"); + writePose(xmlWriter,poses[i]); + xmlWriter->writeEndElement(); + } + xmlWriter->writeEndElement(); // Poses + xmlWriter->writeEndDocument(); + delete xmlWriter; +} + +int readNumberOfViews(std::string xmlFile){ + QFile file(xmlFile.c_str()); + if (!file.exists()){return -1;} + + file.open(QIODevice::ReadOnly); + QXmlStreamReader* xmlReader = new QXmlStreamReader(&file); + int count = 0; + while (!xmlReader->atEnd() && !xmlReader->hasError()){ + QXmlStreamReader::TokenType token = xmlReader->readNext(); + if (token == QXmlStreamReader::StartDocument) + continue; + + if (xmlReader->hasError()){return -1;} + + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "View"){ + count++; + } + } + } + delete xmlReader; + return count; +} + +void writeXml(std::string xmlFile, std::vector & frames, std::vector & poses){ + int slash_pos = xmlFile.find_last_of("/"); + std::string sweep_folder = xmlFile.substr(0, slash_pos) + "/"; + + QFile file(xmlFile.c_str()); + if (file.exists()){file.remove();} + + + if (!file.open(QIODevice::ReadWrite | QIODevice::Text)) + { + std::cerr<<"Could not open file "<setDevice(&file); + + xmlWriter->writeStartDocument(); + xmlWriter->writeStartElement("Views"); + xmlWriter->writeAttribute("number_of_views", QString::number(frames.size())); + for(unsigned int i = 0; i < frames.size(); i++){ + reglib::RGBDFrame * frame = frames[i]; + char buf [1024]; + + xmlWriter->writeStartElement("View"); + sprintf(buf,"%s/view_RGB%10.10i.png",sweep_folder.c_str(),i); + cv::imwrite(buf, frame->rgb ); + + sprintf(buf,"view_RGB%10.10i.png",i); + xmlWriter->writeAttribute("RGB", QString(buf)); + + + sprintf(buf,"%s/view_DEPTH%10.10i.png",sweep_folder.c_str(),i); + cv::imwrite(buf, frame->depth ); + sprintf(buf,"view_DEPTH%10.10i.png",i); + xmlWriter->writeAttribute("DEPTH", QString(buf)); + + long nsec = 1e9*((frame->capturetime)-std::floor(frame->capturetime)); + long sec = frame->capturetime; + xmlWriter->writeStartElement("Stamp"); + + xmlWriter->writeStartElement("sec"); + xmlWriter->writeCharacters(QString::number(sec)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("nsec"); + xmlWriter->writeCharacters(QString::number(nsec)); + xmlWriter->writeEndElement(); + + xmlWriter->writeEndElement(); // Stamp + + xmlWriter->writeStartElement("Camera"); + + xmlWriter->writeStartElement("fx"); + xmlWriter->writeCharacters(QString::number(frame->camera->fx)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("fy"); + xmlWriter->writeCharacters(QString::number(frame->camera->fy)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("cx"); + xmlWriter->writeCharacters(QString::number(frame->camera->cx)); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("cy"); + xmlWriter->writeCharacters(QString::number(frame->camera->cy)); + xmlWriter->writeEndElement(); + + xmlWriter->writeEndElement(); // camera + + xmlWriter->writeStartElement("RegisteredPose"); + quasimodo_brain::writePose(xmlWriter,poses[i]); + xmlWriter->writeEndElement(); + + xmlWriter->writeStartElement("Pose"); + quasimodo_brain::writePose(xmlWriter,frame->pose); + xmlWriter->writeEndElement(); + + xmlWriter->writeEndElement(); + } + xmlWriter->writeEndElement(); // Semantic Room + xmlWriter->writeEndDocument(); + delete xmlWriter; +} + +Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader){ + QXmlStreamReader::TokenType token = xmlReader->readNext();//Translation + QString elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//fx + double tx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//fy + double ty = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//cx + double tz = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Translation + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//Rotation + elementName = xmlReader->name().toString(); + + token = xmlReader->readNext();//qw + double qw = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qx + double qx = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qy + double qy = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//qz + double qz = atof(xmlReader->readElementText().toStdString().c_str()); + + token = xmlReader->readNext();//Rotation + elementName = xmlReader->name().toString(); + + Eigen::Matrix4d regpose = (Eigen::Affine3d(Eigen::Quaterniond(qw,qx,qy,qz))).matrix(); + regpose(0,3) = tx; + regpose(1,3) = ty; + regpose(2,3) = tz; + + return regpose; +} + +void writePose(QXmlStreamWriter* xmlWriter, Eigen::Matrix4d pose){ + Eigen::Quaterniond q = Eigen::Quaterniond ((Eigen::Affine3d (pose)).rotation()); + + xmlWriter->writeStartElement("Translation"); + xmlWriter->writeStartElement("x"); + xmlWriter->writeCharacters(QString::number(pose(0,3))); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("y"); + xmlWriter->writeCharacters(QString::number(pose(1,3))); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("z"); + xmlWriter->writeCharacters(QString::number(pose(2,3))); + xmlWriter->writeEndElement(); + xmlWriter->writeEndElement(); // Translation + + xmlWriter->writeStartElement("Rotation"); + xmlWriter->writeStartElement("w"); + xmlWriter->writeCharacters(QString::number(q.w())); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("x"); + xmlWriter->writeCharacters(QString::number(q.x())); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("y"); + xmlWriter->writeCharacters(QString::number(q.y())); + xmlWriter->writeEndElement(); + xmlWriter->writeStartElement("z"); + xmlWriter->writeCharacters(QString::number(q.z())); + xmlWriter->writeEndElement(); + xmlWriter->writeEndElement(); //Rotation +} + +void remove_old_seg(std::string sweep_folder){ + printf("remove_old_seg: %s\n",sweep_folder.c_str()); + DIR *dir; + struct dirent *ent; + if ((dir = opendir (sweep_folder.c_str())) != NULL) { + /* print all the files and directories within directory */ + while ((ent = readdir (dir)) != NULL) { + std::string file = std::string(ent->d_name); + + if (file.find("dynamic_obj") !=std::string::npos && (file.find(".xml") !=std::string::npos || file.find(".pcd") !=std::string::npos)){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } + + if (file.find("dynamicmask") !=std::string::npos && file.find(".png") !=std::string::npos){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } + + if (file.find("moving_obj") !=std::string::npos && (file.find(".xml") !=std::string::npos || file.find(".pcd") !=std::string::npos)){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } + + if (file.find("movingmask") !=std::string::npos && file.find(".png") !=std::string::npos){ + printf ("removing %s\n", ent->d_name); + std::remove((sweep_folder+"/"+file).c_str()); + } + } + closedir (dir); + } + printf("done remove_old_seg\n"); +} + +std::string replaceAll(std::string str, std::string from, std::string to){ + std::size_t found = str.find(from); + while(found!=std::string::npos){ + str.replace(found,from.size(),to); + found = str.find(from); + } + return str; +} + void cleanPath(std::string & path){ std::size_t found = path.find("//"); while (found!=std::string::npos){ @@ -368,7 +786,7 @@ void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > for(unsigned int i = 0; i < bgs.size(); i++){cpmod[i] = mfrmod.poses[i];} - for(int j = 0; j < models.size(); j++){ + for(unsigned int j = 0; j < models.size(); j++){ Eigen::Matrix4d change = mfrmod.poses[j+bgs.size()]; for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ models[j]->relativeposes[k] = change*models[j]->relativeposes[k]; @@ -378,13 +796,13 @@ void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > } } }else if(models.size() > 1){ - for(int j = 0; j < models.size(); j++){ + for(unsigned int j = 0; j < models.size(); j++){ if(models[j]->points.size() == 0){models[j]->recomputeModelPoints();} cpmod.push_back(models.front()->relativeposes.front().inverse() * models[j]->relativeposes.front()); massregmod->addModel(models[j]); } reglib::MassFusionResults mfrmod = massregmod->getTransforms(cpmod); - for(int j = 0; j < models.size(); j++){ + for(unsigned int j = 0; j < models.size(); j++){ Eigen::Matrix4d change = mfrmod.poses[j] * cpmod[j].inverse(); for(unsigned int k = 0; k < models[j]->relativeposes.size(); k++){ @@ -412,7 +830,7 @@ void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > bgmask.push_back(bgs[i]->modelmasks[k]->getMask()); } } - for(int j = 0; j < models.size(); j++){ + for(unsigned int j = 0; j < models.size(); j++){ reglib::Model * model = models[j]; std::vector masks; diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index 0ee2fe11..552d4d22 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -38,6 +38,32 @@ #include namespace quasimodo_brain { + + +std::vector getSuperPoints(std::string path); + +std::vector getRoomSuperPoints(std::string path, std::string savePath); + +void transformSuperPoints(std::vector & spvec, Eigen::Matrix4d cp); + +void saveSuperPoints(std::string path, std::vector & spvec, Eigen::Matrix4d pose, float ratio_keep = 0.1); + +std::vector readPoseXML(std::string xmlFile); + +void savePoses(std::string xmlFile, std::vector poses, int maxposes = -1); + +Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader); + +int readNumberOfViews(std::string xmlFile); + +void writeXml(std::string xmlFile, std::vector & frames, std::vector & poses); + +void writePose(QXmlStreamWriter* xmlWriter, Eigen::Matrix4d pose); + +void remove_old_seg(std::string sweep_folder); + +std::string replaceAll(std::string str, std::string from, std::string to); + void cleanPath(std::string & path); void sortXMLs(std::vector & sweeps); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 56d9372a..fe79cb7c 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -12,18 +12,13 @@ #include "metaroom_xml_parser/simple_xml_parser.h" #include "metaroom_xml_parser/simple_summary_parser.h" -#include - -#include "ros/ros.h" #include #include -#include #include "metaroom_xml_parser/load_utilities.h" #include #include -#include #include "quasimodo_msgs/model.h" #include "quasimodo_msgs/rgbd_frame.h" @@ -34,26 +29,8 @@ #include "quasimodo_msgs/segment_model.h" #include "quasimodo_msgs/metaroom_pair.h" -#include "ros/ros.h" #include #include -#include -#include -#include - -#include "metaroom_xml_parser/load_utilities.h" -#include -#include - -#include "quasimodo_msgs/model.h" -#include "quasimodo_msgs/rgbd_frame.h" -#include "quasimodo_msgs/model_from_frame.h" -#include "quasimodo_msgs/index_frame.h" -#include "quasimodo_msgs/fuse_models.h" -#include "quasimodo_msgs/get_model.h" - -#include "metaroom_xml_parser/simple_xml_parser.h" -#include "metaroom_xml_parser/load_utilities.h" #include #include @@ -161,258 +138,9 @@ bool testDynamicObjectServiceCallback(std::string path); bool dynamicObjectsServiceCallback(DynamicObjectsServiceRequest &req, DynamicObjectsServiceResponse &res); void processSweepForDatabase(std::string path, std::string savePath); -void signal_callback_handler(int signum){ - printf("Caught signal %d\n",signum); - exit(signum); -} - -std::string replaceAll(std::string str, std::string from, std::string to){ - std::size_t found = str.find(from); - while(found!=std::string::npos){ - str.replace(found,from.size(),to); - found = str.find(from); - } - return str; -} - - -void remove_old_seg(std::string sweep_folder){ - printf("remove_old_seg: %s\n",sweep_folder.c_str()); - DIR *dir; - struct dirent *ent; - if ((dir = opendir (sweep_folder.c_str())) != NULL) { - /* print all the files and directories within directory */ - while ((ent = readdir (dir)) != NULL) { - std::string file = std::string(ent->d_name); - - if (file.find("dynamic_obj") !=std::string::npos && (file.find(".xml") !=std::string::npos || file.find(".pcd") !=std::string::npos)){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); - } - - if (file.find("dynamicmask") !=std::string::npos && file.find(".png") !=std::string::npos){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); - } - - if (file.find("moving_obj") !=std::string::npos && (file.find(".xml") !=std::string::npos || file.find(".pcd") !=std::string::npos)){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); - } - - if (file.find("movingmask") !=std::string::npos && file.find(".png") !=std::string::npos){ - printf ("removing %s\n", ent->d_name); - std::remove((sweep_folder+"/"+file).c_str()); - } - - // if (file.find("_object_") !=std::string::npos && file.find(".xml") !=std::string::npos){ - // printf ("removing %s\n", ent->d_name); - // std::remove((sweep_folder+"/"+file).c_str()); - // } - // if (file.find("_object_") !=std::string::npos && file.find(".pcd") !=std::string::npos){ - // printf ("maybe: removing %s\n", ent->d_name); - // printf("%i\n",file.find("_additional_view_") == std::string::npos); - // //file.find("_additional_view_") == std::string::npos; - // //std::remove((sweep_folder+"/"+file).c_str()); - // } - } - closedir (dir); - } - - printf("done remove_old_seg\n"); -} - -void writePose(QXmlStreamWriter* xmlWriter, Eigen::Matrix4d pose){ - Eigen::Quaterniond q = Eigen::Quaterniond ((Eigen::Affine3d (pose)).rotation()); - - xmlWriter->writeStartElement("Translation"); - xmlWriter->writeStartElement("x"); - xmlWriter->writeCharacters(QString::number(pose(0,3))); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("y"); - xmlWriter->writeCharacters(QString::number(pose(1,3))); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("z"); - xmlWriter->writeCharacters(QString::number(pose(2,3))); - xmlWriter->writeEndElement(); - xmlWriter->writeEndElement(); // Translation - - xmlWriter->writeStartElement("Rotation"); - xmlWriter->writeStartElement("w"); - xmlWriter->writeCharacters(QString::number(q.w())); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("x"); - xmlWriter->writeCharacters(QString::number(q.x())); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("y"); - xmlWriter->writeCharacters(QString::number(q.y())); - xmlWriter->writeEndElement(); - xmlWriter->writeStartElement("z"); - xmlWriter->writeCharacters(QString::number(q.z())); - xmlWriter->writeEndElement(); - xmlWriter->writeEndElement(); //Rotation -} - -void writeXml(std::string xmlFile, std::vector & frames, std::vector & poses){ - int slash_pos = xmlFile.find_last_of("/"); - std::string sweep_folder = xmlFile.substr(0, slash_pos) + "/"; - - QFile file(xmlFile.c_str()); - if (file.exists()){file.remove();} - - - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)) - { - std::cerr<<"Could not open file "<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("Views"); - xmlWriter->writeAttribute("number_of_views", QString::number(frames.size())); - for(unsigned int i = 0; i < frames.size(); i++){ - reglib::RGBDFrame * frame = frames[i]; - char buf [1024]; - - xmlWriter->writeStartElement("View"); - sprintf(buf,"%s/view_RGB%10.10i.png",sweep_folder.c_str(),i); - cv::imwrite(buf, frame->rgb ); - - sprintf(buf,"view_RGB%10.10i.png",i); - xmlWriter->writeAttribute("RGB", QString(buf)); - - - sprintf(buf,"%s/view_DEPTH%10.10i.png",sweep_folder.c_str(),i); - cv::imwrite(buf, frame->depth ); - sprintf(buf,"view_DEPTH%10.10i.png",i); - xmlWriter->writeAttribute("DEPTH", QString(buf)); - - long nsec = 1e9*((frame->capturetime)-std::floor(frame->capturetime)); - long sec = frame->capturetime; - xmlWriter->writeStartElement("Stamp"); - - xmlWriter->writeStartElement("sec"); - xmlWriter->writeCharacters(QString::number(sec)); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("nsec"); - xmlWriter->writeCharacters(QString::number(nsec)); - xmlWriter->writeEndElement(); - - xmlWriter->writeEndElement(); // Stamp - - xmlWriter->writeStartElement("Camera"); - - xmlWriter->writeStartElement("fx"); - xmlWriter->writeCharacters(QString::number(frame->camera->fx)); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("fy"); - xmlWriter->writeCharacters(QString::number(frame->camera->fy)); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("cx"); - xmlWriter->writeCharacters(QString::number(frame->camera->cx)); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("cy"); - xmlWriter->writeCharacters(QString::number(frame->camera->cy)); - xmlWriter->writeEndElement(); - - xmlWriter->writeEndElement(); // camera - - xmlWriter->writeStartElement("RegisteredPose"); - writePose(xmlWriter,poses[i]); - xmlWriter->writeEndElement(); - - xmlWriter->writeStartElement("Pose"); - writePose(xmlWriter,frame->pose); - xmlWriter->writeEndElement(); - - xmlWriter->writeEndElement(); - } - xmlWriter->writeEndElement(); // Semantic Room - xmlWriter->writeEndDocument(); - delete xmlWriter; -} - -Eigen::Matrix4d getPose(QXmlStreamReader * xmlReader){ - QXmlStreamReader::TokenType token = xmlReader->readNext();//Translation - QString elementName = xmlReader->name().toString(); - - token = xmlReader->readNext();//fx - double tx = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//fy - double ty = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//cx - double tz = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//Translation - elementName = xmlReader->name().toString(); - - token = xmlReader->readNext();//Rotation - elementName = xmlReader->name().toString(); - - token = xmlReader->readNext();//qw - double qw = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//qx - double qx = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//qy - double qy = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//qz - double qz = atof(xmlReader->readElementText().toStdString().c_str()); - - token = xmlReader->readNext();//Rotation - elementName = xmlReader->name().toString(); - - Eigen::Matrix4d regpose = (Eigen::Affine3d(Eigen::Quaterniond(qw,qx,qy,qz))).matrix(); - regpose(0,3) = tx; - regpose(1,3) = ty; - regpose(2,3) = tz; - - return regpose; -} - -int readNumberOfViews(std::string xmlFile){ - QFile file(xmlFile.c_str()); - if (!file.exists()){return -1;} - - file.open(QIODevice::ReadOnly); - QXmlStreamReader* xmlReader = new QXmlStreamReader(&file); - - - int count = 0; - while (!xmlReader->atEnd() && !xmlReader->hasError()){ - QXmlStreamReader::TokenType token = xmlReader->readNext(); - if (token == QXmlStreamReader::StartDocument) - continue; - - if (xmlReader->hasError()){return -1;} - - if (token == QXmlStreamReader::StartElement){ - if (xmlReader->name() == "View"){ - count++; - } - } - } - delete xmlReader; - return count; -} - void readViewXML(std::string xmlFile, std::vector & frames, std::vector & poses, bool compute_edges = true, std::string savePath = ""){ - //printf("readViewXML: compute_edges: %i\n",compute_edges); QFile file(xmlFile.c_str()); - - if (!file.exists()) - { + if (!file.exists()){ ROS_ERROR("Could not open file %s to load room.",xmlFile.c_str()); return; } @@ -421,57 +149,37 @@ void readViewXML(std::string xmlFile, std::vector & frames, int index = xmlFileQS.lastIndexOf('/'); std::string roomFolder = xmlFileQS.left(index).toStdString(); - file.open(QIODevice::ReadOnly); ROS_INFO_STREAM("Parsing xml file: "<atEnd() && !xmlReader->hasError()) - { + while (!xmlReader->atEnd() && !xmlReader->hasError()){ QXmlStreamReader::TokenType token = xmlReader->readNext(); if (token == QXmlStreamReader::StartDocument) continue; - if (xmlReader->hasError()) - { + if (xmlReader->hasError()){ ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); return; } QString elementName = xmlReader->name().toString(); - if (token == QXmlStreamReader::StartElement) - { - - if (xmlReader->name() == "View") - { + if (token == QXmlStreamReader::StartElement){ + if (xmlReader->name() == "View"){ cv::Mat rgb; cv::Mat depth; //printf("elementName: %s\n",elementName.toStdString().c_str()); QXmlStreamAttributes attributes = xmlReader->attributes(); - if (attributes.hasAttribute("RGB")) - { + if (attributes.hasAttribute("RGB")){ std::string imgpath = attributes.value("RGB").toString().toStdString(); - printf("rgb filename: %s\n",(roomFolder+"/"+imgpath).c_str()); rgb = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); - - //QString rgbpath = attributes.value("RGB").toString(); - //rgb = cv::imread(roomFolder+"/"+(rgbpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); - //rgb = cv::imread((rgbpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); }else{break;} - - if (attributes.hasAttribute("DEPTH")) - { + if (attributes.hasAttribute("DEPTH")){ std::string imgpath = attributes.value("DEPTH").toString().toStdString(); - printf("depth filename: %s\n",(roomFolder+"/"+imgpath).c_str()); depth = cv::imread(roomFolder+"/"+imgpath, CV_LOAD_IMAGE_UNCHANGED); - //QString depthpath = attributes.value("DEPTH").toString(); - //printf("depth filename: %s\n",depthpath.toStdString().c_str()); - //depth = cv::imread((roomFolder+"/"+depthpath.toStdString()).c_str(), CV_LOAD_IMAGE_UNCHANGED); - //depth = cv::imread(roomFolder+"/"+(depthpath.toStdString().c_str()), CV_LOAD_IMAGE_UNCHANGED); }else{break;} @@ -512,7 +220,7 @@ void readViewXML(std::string xmlFile, std::vector & frames, token = xmlReader->readNext();//RegisteredPose elementName = xmlReader->name().toString(); - Eigen::Matrix4d regpose = getPose(xmlReader); + Eigen::Matrix4d regpose = quasimodo_brain::getPose(xmlReader); token = xmlReader->readNext();//RegisteredPose elementName = xmlReader->name().toString(); @@ -521,7 +229,7 @@ void readViewXML(std::string xmlFile, std::vector & frames, token = xmlReader->readNext();//Pose elementName = xmlReader->name().toString(); - Eigen::Matrix4d pose = getPose(xmlReader); + Eigen::Matrix4d pose = quasimodo_brain::getPose(xmlReader); token = xmlReader->readNext();//Pose elementName = xmlReader->name().toString(); @@ -645,7 +353,7 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); } - //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( sweep, reg); mu->occlusion_penalty = 15; @@ -653,7 +361,6 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri mu->viewer = viewer; sweep->recomputeModelPoints(); - //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); reglib::Model * fullmodel = new reglib::Model(); fullmodel->savePath = savePath+"/"; fullmodel->frames = sweep->frames; @@ -743,103 +450,297 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri return fullmodel; } -void savePoses(std::string xmlFile, std::vector poses, int maxposes = -1){ - QFile file(xmlFile.c_str()); - if (file.exists()){file.remove();} +reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std::string saveVisuals_sp = ""){ + printf("getAVMetaroom: %s\n",path.c_str()); - if (!file.open(QIODevice::ReadWrite | QIODevice::Text)){ - std::cerr<<"Could not open file "<< xmlFile <<" to save views as XML"<setDevice(&file); - - xmlWriter->writeStartDocument(); - xmlWriter->writeStartElement("Poses"); - // xmlWriter->writeAttribute("number_of_poses", QString::number(poses.size())); - for(unsigned int i = 0; i < poses.size() && (maxposes == -1 || int(i) < maxposes); i++){ - printf("saving %i\n",i); - xmlWriter->writeStartElement("Pose"); - writePose(xmlWriter,poses[i]); - xmlWriter->writeEndElement(); - } - xmlWriter->writeEndElement(); // Poses - xmlWriter->writeEndDocument(); - delete xmlWriter; -} + int slash_pos = path.find_last_of("/"); + std::string sweep_folder = path.substr(0, slash_pos) + "/"; -std::vector readPoseXML(std::string xmlFile){ - std::vector poses; - QFile file(xmlFile.c_str()); + int viewgroup_nrviews = quasimodo_brain::readNumberOfViews(sweep_folder+"ViewGroup.xml"); - if (!file.exists()){ - ROS_ERROR("Could not open file %s to load poses.",xmlFile.c_str()); - return poses; + printf("sweep_folder: %s\n",sweep_folder.c_str()); + + int additional_nrviews = 0; + QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); + for (auto objectFile : objectFiles){ + printf("objectFile: %s\n",(sweep_folder+objectFile.toStdString()).c_str()); + auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); + additional_nrviews += object.vAdditionalViews.size(); } - file.open(QIODevice::ReadOnly); - ROS_INFO_STREAM("Parsing xml file: "< parser; + SimpleXMLParser::RoomData roomData = parser.loadRoomFromXML(path,std::vector{"RoomIntermediateCloud","IntermediatePosition"}); + int metaroom_nrviews = roomData.vIntermediateRoomClouds.size(); - QXmlStreamReader* xmlReader = new QXmlStreamReader(&file); - while (!xmlReader->atEnd() && !xmlReader->hasError()){ - QXmlStreamReader::TokenType token = xmlReader->readNext(); - if (token == QXmlStreamReader::StartDocument) - continue; + printf("viewgroup_nrviews: %i\n",viewgroup_nrviews); + printf("additional_nrviews: %i\n",additional_nrviews); + printf("metaroom_nrviews: %i\n",metaroom_nrviews); - if (xmlReader->hasError()){ - ROS_ERROR("XML error: %s",xmlReader->errorString().toStdString().c_str()); - return poses; + std::vector frames; + + char buf [1024]; + int counter = 0; + while(true){//Load all frames that can be loaded + sprintf(buf,"%s/frame_%5.5i",sweep_folder.c_str(),counter); + string tmp = string(buf)+"_data.txt"; + QFile file(tmp.c_str()); + if (!file.exists()){ + break; } - QString elementName = xmlReader->name().toString(); + reglib::Camera * cam = reglib::Camera::load(std::string(buf)+"_camera"); + if(cam == 0){break;} + cam->print(); - if (token == QXmlStreamReader::StartElement){ - if (xmlReader->name() == "Pose"){ - Eigen::Matrix4d pose = getPose(xmlReader); + reglib::RGBDFrame * frame = reglib::RGBDFrame::load(cam,std::string(buf)); + if(frame == 0){delete cam; break;} + frames.push_back(frame); - token = xmlReader->readNext();//Pose - elementName = xmlReader->name().toString(); + counter++; + } - std::cout << pose << std::endl << std::endl; - poses.push_back(pose); - } - } + printf("frames: %i counter: %i\n",frames.size(),counter); + + + if(roomData.vIntermediateRoomClouds.size() == 0 ){ + printf("WARNING:: no clouds in room\n"); } - delete xmlReader; - printf("done readPoseXML\n"); - return poses; -} -reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std::string saveVisuals_sp = ""){ - printf("processing: %s\n",path.c_str()); + Eigen::Matrix4d m2 = Eigen::Matrix4d ::Identity(); + if(roomData.vIntermediateRoomClouds.size() != 0 ){m2 = quasimodo_brain::getMat(roomData.vIntermediateRoomCloudTransforms[0]);} + + + reglib::Model * sweepmodel = 0; + + cv::Mat fullmask; + fullmask.create(480,640,CV_8UC1); + unsigned char * maskdata = (unsigned char *)fullmask.data; + for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} + + std::vector metaroom_frames; + for (size_t i=0; i= counter){ + image_geometry::PinholeCameraModel aCameraModel = roomData.vIntermediateRoomCloudCamParamsCorrected[i]; + //image_geometry::PinholeCameraModel aCameraModel = roomData.vIntermediateRoomCloudCamParams[i]; + reglib::Camera * cam = new reglib::Camera(); + cam->fx = aCameraModel.fx(); + cam->fy = aCameraModel.fy(); + cam->cx = aCameraModel.cx(); + cam->cy = aCameraModel.cy(); + + Eigen::Matrix4d m = m2*quasimodo_brain::getMat(roomData.vIntermediateRoomCloudTransformsRegistered[i]); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,roomData.vIntermediateRGBImages[i],5.0*roomData.vIntermediateDepthImages[i],0, m,true,saveVisuals_sp); + + sprintf(buf,"%s/frame_%5.5i",sweep_folder.c_str(),counter); + frame->save(std::string(buf)); + cam->save(std::string(buf)+"_camera"); + metaroom_frames.push_back(frame); + counter++; + }else{ + metaroom_frames.push_back(frames[i]); + } - if ( ! boost::filesystem::exists( path ) ){return 0;} + metaroom_frames.back()->camera->print(); - int slash_pos = path.find_last_of("/"); - std::string sweep_folder = path.substr(0, slash_pos) + "/"; + if(sweepmodel == 0){ + sweepmodel = new reglib::Model(metaroom_frames.back(),fullmask); + }else{ + sweepmodel->frames.push_back(metaroom_frames.back()); + sweepmodel->relativeposes.push_back(metaroom_frames.front()->pose.inverse() * metaroom_frames.back()->pose); + sweepmodel->modelmasks.push_back(new reglib::ModelMask(fullmask)); + } + } + //Metaroom frames added... - int viewgroup_nrviews = readNumberOfViews(sweep_folder+"ViewGroup.xml"); + sweepmodel->points = quasimodo_brain::getSuperPoints(sweep_folder+"/sweepmodel_superpoints.bin");//Try to load superpoints + if(sweepmodel->points.size() == 0){ //If it fails we have to recompute the points... + sweepmodel->recomputeModelPoints(); + quasimodo_brain::saveSuperPoints(sweep_folder+"/sweepmodel_superpoints.bin",sweepmodel->points,Eigen::Matrix4d::Identity(),1.0); + } + printf("sweepmodel fully loaded!"); + //Load rest of model if possible - printf("sweep_folder: %s\n",sweep_folder.c_str()); + std::vector viewrgbs; + std::vector viewdepths; + std::vector viewtfs; - int additional_nrviews = 0; QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); for (auto objectFile : objectFiles){ - printf("objectFile: %s\n",(sweep_folder+objectFile.toStdString()).c_str()); auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); - additional_nrviews += object.vAdditionalViews.size(); - } + for (unsigned int i=0; i parser; - SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path,std::vector{"RoomIntermediateCloud","IntermediatePosition"}); - int metaroom_nrviews = current_roomData.vIntermediateRoomClouds.size(); + counter++; + CloudPtr cloud = object.vAdditionalViews[i]; - printf("viewgroup_nrviews: %i\n",viewgroup_nrviews); - printf("additional_nrviews: %i\n",additional_nrviews); - printf("metaroom_nrviews: %i\n",metaroom_nrviews); + cv::Mat rgb; + rgb.create(cloud->height,cloud->width,CV_8UC3); + unsigned char * rgbdata = (unsigned char *)rgb.data; + + cv::Mat depth; + depth.create(cloud->height,cloud->width,CV_16UC1); + unsigned short * depthdata = (unsigned short *)depth.data; + + unsigned int nr_data = cloud->height * cloud->width; + for(unsigned int j = 0; j < nr_data; j++){ + PointType p = cloud->points[j]; + rgbdata[3*j+0] = p.b; + rgbdata[3*j+1] = p.g; + rgbdata[3*j+2] = p.r; + depthdata[j] = short(5000.0 * p.z); + } + + viewrgbs.push_back(rgb); + viewdepths.push_back(depth); + viewtfs.push_back(object.vAdditionalViewsTransforms[i]); + } + } + + +// for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ +// sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; +// sweep->relativeposes[i] = sweepPoses[i]; +// if(basecam != 0){ +// delete sweep->frames[i]->camera; +// sweep->frames[i]->camera = basecam->clone(); +// } +// } + +// std::vector frames; +// std::vector masks; +// std::vector unrefined; + +// std::vector both_unrefined; +// both_unrefined.push_back(Eigen::Matrix4d::Identity()); +// std::vector times; +// for(unsigned int i = 0; i < 3000 && i < viewrgbs.size(); i++){ +// printf("additional view: %i\n",i); +// geometry_msgs::TransformStamped msg; +// tf::transformStampedTFToMsg(viewtfs[i], msg); +// long sec = msg.header.stamp.sec; +// long nsec = msg.header.stamp.nsec; +// double time = double(sec)+1e-9*double(nsec); + +// Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]); + +// cout << m << endl << endl; + +// unrefined.push_back(m); +// times.push_back(time); + +// cv::Mat fullmask; +// fullmask.create(480,640,CV_8UC1); +// unsigned char * maskdata = (unsigned char *)fullmask.data; +// for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} +// masks.push_back(new reglib::ModelMask(fullmask)); + +// reglib::Camera * cam = sweep->frames.front()->camera->clone(); +// reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m,true,savePath);//a.matrix()); +// frames.push_back(frame); + +// both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); +// } + +// reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); +// reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( sweep, reg); +// mu->occlusion_penalty = 15; +// mu->massreg_timeout = 60*4; +// mu->viewer = viewer; + +// sweep->recomputeModelPoints(); +// reglib::Model * fullmodel = new reglib::Model(); +// fullmodel->savePath = savePath+"/"; +// fullmodel->frames = sweep->frames; +// fullmodel->relativeposes = sweep->relativeposes; +// fullmodel->modelmasks = sweep->modelmasks; +// //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); +// if(frames.size() > 0){ + +// reglib::RegistrationRefinement * refinement = new reglib::RegistrationRefinement(); +// refinement->viewer = viewer; +// refinement->visualizationLvl = visualization_lvl_regini; +// refinement->normalize_matchweights = false; +// refinement->target_points = 4000; +// ////double register_setup_start = getTime(); + +// //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); +// reglib::CloudData * cd1 = sweep->getCD(sweep->points.size()); +// refinement->setDst(cd1); + +// fullmodel->points = frames.front()->getSuperPoints(Eigen::Matrix4d::Identity(),1,false); +// reglib::CloudData * cd2 = fullmodel->getCD(fullmodel->points.size()); +// refinement->setSrc(cd2); +// //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); +// //////printf("register_setup_start: %5.5f\n",getTime()-register_setup_start); +// ////double register_compute_start = getTime(); +// Eigen::Matrix4d guess = both_unrefined[1]*both_unrefined[0].inverse(); + +// reglib::FusionResults fr = refinement->getTransform(guess); +// Eigen::Matrix4d offset = fr.guess*guess.inverse(); +// for(unsigned int i = 1; i < both_unrefined.size(); i++){ +// both_unrefined[i] = offset*both_unrefined[i]; +// } + +// delete refinement; + +// fullmodel->points.clear(); +// delete cd1; +// delete cd2; + +// reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.25); +// if(savePath.size() != 0){ +// bgmassreg->savePath = savePath+"/processAV_"+std::to_string(fullmodel->id); +// } +// //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); + +// bgmassreg->timeout = 300; +// bgmassreg->viewer = viewer; +// bgmassreg->use_surface = true; +// bgmassreg->use_depthedge = false; +// bgmassreg->visualizationLvl = visualization_lvl_regref; +// bgmassreg->maskstep = 5; +// bgmassreg->nomaskstep = 5; +// bgmassreg->nomask = true; +// bgmassreg->stopval = 0.0005; +// bgmassreg->addModel(sweep); +// bgmassreg->setData(frames,masks); +// // exit(0); +// //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); + +// reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); + +// delete bgmassreg; + +// for(unsigned int i = 0; i < frames.size(); i++){frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1];} + +// for(unsigned int i = 0; i < frames.size(); i++){ +// fullmodel->frames.push_back(frames[i]); +// fullmodel->modelmasks.push_back(masks[i]); +// fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); +// } +// fullmodel->recomputeModelPoints(); + +// if(savePath.size() != 0){ +// pcl::PointCloud::Ptr cld = fullmodel->getPCLnormalcloud(1, false); +// pcl::io::savePCDFileBinaryCompressed (savePath+"/processAV_"+std::to_string(fullmodel->id)+"_fused.pcd", *cld); +// } +// //pcl::PointCloud::Ptr Model::getPCLnormalcloud(int step, bool color){ + +// }else{ +// fullmodel->points = sweep->points; +// } + +// delete reg; +// delete mu; +// delete sweep; + + + //Load the unloaded frames... +exit(0); reglib::Model * fullmodel; if(viewgroup_nrviews == (additional_nrviews+metaroom_nrviews) && !recomputeRelativePoses){ @@ -861,7 +762,7 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); fullmodel = processAV(path,compute_edges,saveVisuals_sp); //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); - writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); + quasimodo_brain::writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); } //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); @@ -872,7 +773,7 @@ int totalcounter = 0; int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = true){ - path = replaceAll(path, "//", "/"); + path = quasimodo_brain::replaceAll(path, "//", "/"); quasimodo_brain::cleanPath(path); int returnval = 0; @@ -883,18 +784,14 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr if ( ! boost::filesystem::exists( path ) ){return 0;} - - - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); store_old_xml = objectFiles.size() == 0; SimpleXMLParser parser; - - SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path,std::vector()); + //SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path,std::vector()); // if(current_roomData.roomWaypointId.compare("ReceptionDesk") != 0){return 0;} - current_roomData = parser.loadRoomFromXML(path,std::vector{"RoomIntermediateCloud","IntermediatePosition"}); + SimpleXMLParser::RoomData current_roomData = parser.loadRoomFromXML(path,std::vector{"RoomIntermediateCloud","IntermediatePosition"}); if(current_roomData.vIntermediateRoomClouds.size() != 17){return 0;} @@ -928,7 +825,7 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr int prevind = -1; std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); for (unsigned int i = 0; i < sweep_xmls.size(); i++){ - sweep_xmls[i] = replaceAll(sweep_xmls[i], "//", "/"); + sweep_xmls[i] = quasimodo_brain::replaceAll(sweep_xmls[i], "//", "/"); SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); std::string other_waypointid = other_roomData.roomWaypointId; @@ -1001,7 +898,7 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr quasimodo_brain::segment(bgs,models,internal,external,dynamic,visualization_lvl,saveVisuals); - remove_old_seg(sweep_folder); + quasimodo_brain::remove_old_seg(sweep_folder); if(models.size() == 0){ returnval = 2;} else{ returnval = 3;} @@ -1398,7 +1295,7 @@ void trainMetaroom(std::string path){ bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); exit(0); - savePoses(overall_folder+"/"+posepath,bgmfr.poses,17); + quasimodo_brain::savePoses(overall_folder+"/"+posepath,bgmfr.poses,17); fullmodel->fullDelete(); delete fullmodel; delete bgmassreg; @@ -1660,49 +1557,6 @@ bool testDynamicObjectServiceCallback(std::string path){ return dynamicObjectsServiceCallback(req,res); } - - -cv::Mat getMaskImg(cv::Mat image, cv::Mat mask){ - unsigned int width = image.rows; - unsigned int height = image.cols; - int maxw = 0; - int minw = width; - int maxh = 0; - int minh = height; - for(int w = 0; w < width; w++){ - for(int h = 0; h < height; h++){ - if(mask.data[h*width+w] != 0){ - maxw = std::max(maxw,w); - minw = std::min(minw,w); - maxh = std::max(maxh,h); - minh = std::min(minh,h); - } - } - } - - double ratio = 0.15; - int diffw = maxw-minw; - int diffh = maxh-minh; - - maxw = std::min(int(width-1 ),int(maxw+ratio*diffw)); - minw = std::max(int(0 ),int(minw-ratio*diffw)); - maxh = std::min(int(height-1),int(maxh+ratio*diffh)); - minh = std::max(int(0 ),int(minh-ratio*diffh)); - - diffw = maxw-minw; - diffh = maxh-minh; - - cv::Rect myROI(minw, minh, diffw, diffh); - cv::Mat localimg = image(myROI); - return localimg; -} - -geometry_msgs::Pose getPoseMSG(Eigen::Matrix4d pose){ - geometry_msgs::Pose pose1; - tf::poseEigenToMsg (Eigen::Affine3d(pose), pose1); - return pose1; -} - void sendRoomToSomaLLSD(std::string path){ printf("sendRoomToSomaLLSD(%s)\n",path.c_str()); @@ -1812,7 +1666,7 @@ void sendRoomToSomaLLSD(std::string path){ token = xmlReader->readNext();//RegisteredPose elementName = xmlReader->name().toString(); - Eigen::Matrix4d regpose = getPose(xmlReader); + Eigen::Matrix4d regpose = quasimodo_brain::getPose(xmlReader); regposes.push_back(regpose); token = xmlReader->readNext();//RegisteredPose @@ -1822,7 +1676,7 @@ void sendRoomToSomaLLSD(std::string path){ token = xmlReader->readNext();//Pose elementName = xmlReader->name().toString(); - Eigen::Matrix4d pose = getPose(xmlReader); + Eigen::Matrix4d pose = quasimodo_brain::getPose(xmlReader); token = xmlReader->readNext();//Pose elementName = xmlReader->name().toString(); @@ -1944,7 +1798,7 @@ void sendRoomToSomaLLSD(std::string path){ if (attributes.hasAttribute("image_number")){ QString depthpath = attributes.value("image_number").toString(); number = atoi(depthpath.toStdString().c_str()); - printf("number: %i\n",number); + //printf("number: %i\n",number); }else{break;} scid.push_back(scenes[number].id); @@ -1957,8 +1811,11 @@ void sendRoomToSomaLLSD(std::string path){ soma_llsd_msgs::Segment segment; if(quasimodo_conversions::add_masks_to_soma_segment(*np,scid, masks, poses,segment)){ + std::string id = segment.id; for(unsigned int i = 0; i < soma_segment_id_pubs.size(); i++){ - //soma_segment_id_pubs[i].publish(segment.id); + std_msgs::String msg; + msg.data = id; + soma_segment_id_pubs[i].publish(msg); } } } @@ -1988,11 +1845,11 @@ void processSweep(std::string path, std::string savePath){ for(unsigned int i = 0; i < m_PublisherStatuss.size(); i++){m_PublisherStatuss[i].publish(msg);} //Send the previous room to the modelserver... - path = replaceAll(path, "//", "/"); + path = quasimodo_brain::replaceAll(path, "//", "/"); int prevind = -1; std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); for (unsigned int i = 0; i < sweep_xmls.size(); i++){ - sweep_xmls[i] = replaceAll(sweep_xmls[i], "//", "/"); + sweep_xmls[i] = quasimodo_brain::replaceAll(sweep_xmls[i], "//", "/"); if(sweep_xmls[i].compare(path) == 0){break;} prevind = i; } @@ -2013,129 +1870,8 @@ void roomObservationCallback(const semantic_map_msgs::RoomObservationConstPtr& o //processMetaroom } -std::vector getRoomSuperPoints(std::string path, std::string savePath){ - std::vector spvec; - int slash_pos = path.find_last_of("/"); - std::string sweep_folder = path.substr(0, slash_pos) + "/"; - - std::streampos size; - char * memblock; - - std::ifstream file (sweep_folder+"/superpoints.bin", ios::in|ios::binary|ios::ate); - if (file.is_open()) { - size = file.tellg(); - if(size == 0){return spvec;} - - memblock = new char [size]; - file.seekg (0, ios::beg); - file.read (memblock, size); - file.close(); - - cout << "the entire file content is in memory"; - - long nr_points = size / (sizeof(float)*12); - printf("loading %i points\n",nr_points); - - float * data = (float *)memblock; - - spvec.resize(nr_points); - long count = 0; - for(unsigned long i = 0; i < nr_points; i++){ - reglib::superpoint & p = spvec[i]; - p.point(0) = data[count++]; - p.point(1) = data[count++]; - p.point(2) = data[count++]; - p.normal(0) = data[count++]; - p.normal(1) = data[count++]; - p.normal(2) = data[count++]; - p.feature(0) = data[count++]; - p.feature(1) = data[count++]; - p.feature(2) = data[count++]; - p.point_information = data[count++]; - p.normal_information = data[count++]; - p.feature_information = data[count++]; - } - - delete[] memblock; - }else{ - std::cout << "Unable to open file superpoint file, go process that data" << std::endl; - } - - return spvec; -} - -void transformSuperPoints(std::vector & spvec, Eigen::Matrix4d cp){ - float m00 = cp(0,0); float m01 = cp(0,1); float m02 = cp(0,2); float m03 = cp(0,3); - float m10 = cp(1,0); float m11 = cp(1,1); float m12 = cp(1,2); float m13 = cp(1,3); - float m20 = cp(2,0); float m21 = cp(2,1); float m22 = cp(2,2); float m23 = cp(2,3); - - for(unsigned long i = 0; i < spvec.size(); i++){ - reglib::superpoint & p = spvec[i]; - - float x = p.point(0); - float y = p.point(1); - float z = p.point(2); - float nx = p.normal(0); - float ny = p.normal(1); - float nz = p.normal(2); - - float tx = m00*x + m01*y + m02*z + m03; - float ty = m10*x + m11*y + m12*z + m13; - float tz = m20*x + m21*y + m22*z + m23; - - float tnx = m00*nx + m01*ny + m02*nz; - float tny = m10*nx + m11*ny + m12*nz; - float tnz = m20*nx + m21*ny + m22*nz; - - p.point(0) = tx; - p.point(1) = ty; - p.point(2) = tz; - p.normal(0) = tnx; - p.normal(1) = tny; - p.normal(2) = tnz; - } -} - -void saveSuperPoints(std::string path, std::vector & spvec, Eigen::Matrix4d pose, float ratio_keep = 0.1){ - printf("saveSuperPoints(%s)\n",path.c_str()); - transformSuperPoints(spvec,pose); - //XYZ RGB NXNYNZ W1 W2 - long sizeofSuperPoint = 3*(3+1); - - std::ofstream file; - file.open(path, ios::out | ios::binary); - if(spvec.size() != 0){ - float * data = new float[spvec.size() * sizeofSuperPoint]; - long added = 0; - long count = 0; - for(unsigned long i = 0; i < spvec.size(); i++){ - if(double(rand() % 1000)*0.001 > ratio_keep ){continue;} - reglib::superpoint & p = spvec[i]; - data[count++] = p.point(0); - data[count++] = p.point(1); - data[count++] = p.point(2); - data[count++] = p.normal(0); - data[count++] = p.normal(1); - data[count++] = p.normal(2); - data[count++] = p.feature(0); - data[count++] = p.feature(1); - data[count++] = p.feature(2); - data[count++] = p.point_information; - data[count++] = p.normal_information; - data[count++] = p.feature_information; - added++; - } - if(added > 0){ - printf("saving %i points\n",added); - file.write( (char*)data, added*sizeofSuperPoint*sizeof(float)); - } - delete[] data; - } - file.close(); -} - void processSweepForDatabase(std::string path, std::string savePath){ - path = replaceAll(path, "//", "/"); + path = quasimodo_brain::replaceAll(path, "//", "/"); if ( ! boost::filesystem::exists( path ) ){return;} int slash_pos = path.find_last_of("/"); @@ -2163,7 +1899,7 @@ void processSweepForDatabase(std::string path, std::string savePath){ int prevind = -1; std::vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder,false); for (unsigned int i = 0; i < sweep_xmls.size(); i++){ - sweep_xmls[i] = replaceAll(sweep_xmls[i], "//", "/"); + sweep_xmls[i] = quasimodo_brain::replaceAll(sweep_xmls[i], "//", "/"); SimpleXMLParser::RoomData other_roomData = parser.loadRoomFromXML(sweep_xmls[i],std::vector(),false,false); //printf("sweep: %s\n",sweep_xmls[i].c_str()); @@ -2190,9 +1926,9 @@ void processSweepForDatabase(std::string path, std::string savePath){ Eigen::Matrix4d pose = Eigen::Matrix4d::Identity(); Eigen::Matrix4d change = Eigen::Matrix4d::Identity(); if(firstind != -1){ - std::vector backgroundsp = getRoomSuperPoints(sweep_xmls[firstind],savePath); + std::vector backgroundsp = quasimodo_brain::getRoomSuperPoints(sweep_xmls[firstind],savePath); if(prevind != -1 && prevind != firstind){ - std::vector prev_vec = getRoomSuperPoints(sweep_xmls[prevind],savePath); + std::vector prev_vec = quasimodo_brain::getRoomSuperPoints(sweep_xmls[prevind],savePath); backgroundsp.insert(backgroundsp.end(), prev_vec.begin(), prev_vec.end()); } @@ -2239,7 +1975,7 @@ void processSweepForDatabase(std::string path, std::string savePath){ } } - saveSuperPoints(sweep_folder+"/superpoints.bin",sweep->points,pose,1); + quasimodo_brain::saveSuperPoints(sweep_folder+"/superpoints.bin",sweep->points,pose,1); for(unsigned int i = 0; i < sweep->frames.size(); i++){ sweep->frames[i]->pose *= change; } @@ -2424,6 +2160,119 @@ bool testPath(std::string path){ return false; } +void addSceneToLastMetaroom(std::string soma_id){ + printf("void addSceneToLastMetaroom(%s)\n",soma_id.c_str()); + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + std::string lastSweep = sweep_xmls.back(); + + int slash_pos = lastSweep.find_last_of("/"); + std::string sweep_folder = lastSweep.substr(0, slash_pos) + "/"; + + printf("last_sweep: %s\n",sweep_folder.c_str()); + + ros::ServiceClient client = np->serviceClient("/soma_llsd/get_scene"); + ROS_INFO("Waiting for /soma_llsd/get_scene service..."); + if (!client.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/get_scene service!"); +// return; + } + ROS_INFO("Got /soma_llsd/get_scene service"); + + soma_llsd::GetScene srv; + srv.request.scene_id = soma_id; + if (!client.call(srv)) { + ROS_ERROR("Failed to call service /soma_llsd/get_scene"); + } + ROS_INFO("Got /soma_llsd/get_scene"); + + cv_bridge::CvImagePtr rgb_ptr; + try{ rgb_ptr = cv_bridge::toCvCopy(srv.response.response.rgb_img, sensor_msgs::image_encodings::BGR8);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat rgb = rgb_ptr->image; + + cv_bridge::CvImagePtr depth_ptr; + try{ depth_ptr = cv_bridge::toCvCopy(srv.response.response.depth_img, sensor_msgs::image_encodings::MONO16);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat depth = depth_ptr->image; + + + //tf::poseEigenToMsg(Eigen::Affine3d(pose), scene_srv.request.robot_pose); + sensor_msgs::CameraInfo info = srv.response.response.camera_info; + + Eigen::Affine3d epose; + tf::poseMsgToEigen(srv.response.response.robot_pose, epose); + + reglib::Camera * cam = new reglib::Camera(); + if(info.K[0] > 0){ + cam->fx = info.K[0]; + cam->fy = info.K[4]; + cam->cx = info.K[2]; + cam->cy = info.K[5]; + } + + char buf [1024]; + int counter = 0; + while(true){ + sprintf(buf,"%s/frame_%5.5i",sweep_folder.c_str(),counter); + string tmp = string(buf)+"_data.txt"; + QFile file(tmp.c_str()); + if (!file.exists()){break;} + counter++; + } + + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, 0, epose.matrix(),true,"",true); + frame->soma_id = soma_id; + frame->save(std::string(buf)); + cam->save(std::string(buf)+"_camera"); + delete frame; + delete cam; + +// reglib::Camera * cam2 = reglib::Camera::load(std::string(buf)+"_camera"); +// cam2->print(); + +// reglib::RGBDFrame * frame2 = reglib::RGBDFrame::load(cam2,std::string(buf)); +// frame2->show(true); + +// delete cam2; +// delete frame2; + + + //reglib::RGBDFrame * loadedframe = reglib::RGBDFrame::load(); + + //frame->show(true); + //model->frames.push_back(frame); + +// info.K.at(0) = K(0, 0); +// info.K.at(1) = K(0, 1); +// info.K.at(2) = K(0, 2); +// info.K.at(3) = K(1, 0); +// info.K.at(4) = K(1, 1); +// info.K.at(5) = K(1, 2); +// info.K.at(8) = K(2, 2); + +// info.P.at(0) = K(0, 0); +// info.P.at(1) = K(0, 1); +// info.P.at(2) = K(0, 2); +// info.P.at(4) = K(1, 0); +// info.P.at(5) = K(1, 1); +// info.P.at(6) = K(1, 2); +// info.P.at(10) = K(2, 2); +// info.height = rgb.rows; +// info.width = rgb.cols; +// scene_srv.request.camera_info = info; +// scene_srv.request.waypoint = waypoint; +// scene_srv.request.episode_id = episode_id; + +// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); +// //cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); cv::imshow( "normals", normals ); +// cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", depth ); +// //cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges ); +// cv::waitKey(0); +// //if(stop){ cv::waitKey(0);}else{ cv::waitKey(30);} + +// exit(0); +} + void setLargeStack(){ const rlim_t kStackSize = 256 * 1024 * 1024; // min stack size = 256 MB @@ -2512,7 +2361,7 @@ int main(int argc, char** argv){ }else if(inputstate == 6){ posepath = std::string(argv[i]); }else if(inputstate == 7){ - sweepPoses = readPoseXML(std::string(argv[i])); + sweepPoses = quasimodo_brain::readPoseXML(std::string(argv[i])); }else if(inputstate == 8){ sendMetaroomToServers.push_back(std::string(argv[i])); }else if(inputstate == 9){ @@ -2591,6 +2440,10 @@ int main(int argc, char** argv){ printf("overall_folder: %s\n",overall_folder.c_str()); + + //addSceneToLastMetaroom("ed5f22eb-e6c0-426c-b905-4800780ca596"); + + printf("testpaths: %i\n",testpaths.size()); for(unsigned int i = 0; i < testpaths.size(); i++){ testPath(testpaths[i]); diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 5c0cd196..2160c2ba 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -740,6 +740,13 @@ void addNewModel(reglib::Model * model){ dumpDatabase(savepath); } + +void somaCallback(const std_msgs::String & m){ + printf("somaCallback(%s)\n",m.data.c_str()); +// quasimodo_msgs::model mod = m; +// addNewModel(quasimodo_brain::getModelFromMSG(mod,false)); +} + void modelCallback(const quasimodo_msgs::model & m){ quasimodo_msgs::model mod = m; addNewModel(quasimodo_brain::getModelFromMSG(mod,false)); @@ -1315,6 +1322,7 @@ int main(int argc, char **argv){ std::vector input_model_subs; + std::vector soma_input_model_subs; std::vector modelpcds; int inputstate = -1; @@ -1409,6 +1417,11 @@ int main(int argc, char **argv){ //if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/model/out/topic", 100, modelCallback));} if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/quasimodo/segmentation/out/model", 100, modelCallback));} + + + if(soma_input_model_subs.size() == 0){soma_input_model_subs.push_back(n.subscribe("/quasimodo/segmentation/out/soma_segment_id", 10000, somaCallback));} + + if(visualization){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("Modelserver Viewer")); viewer->addCoordinateSystem(0.1); diff --git a/quasimodo/quasimodo_models/include/core/RGBDFrame.h b/quasimodo/quasimodo_models/include/core/RGBDFrame.h index fbe16377..4564d923 100644 --- a/quasimodo/quasimodo_models/include/core/RGBDFrame.h +++ b/quasimodo/quasimodo_models/include/core/RGBDFrame.h @@ -37,6 +37,7 @@ namespace reglib double capturetime; Eigen::Matrix4d pose; int sweepid; + std::string soma_id; //float * rgbdata; cv::Mat det_dilate; diff --git a/quasimodo/quasimodo_models/include/core/Util.h b/quasimodo/quasimodo_models/include/core/Util.h index 051c91f5..f63fb10d 100644 --- a/quasimodo/quasimodo_models/include/core/Util.h +++ b/quasimodo/quasimodo_models/include/core/Util.h @@ -40,6 +40,7 @@ #include "superpoint.h" #include "ReprojectionResult.h" #include "OcclusionScore.h" +#include "quasimodo_point.h" using ceres::NumericDiffCostFunction; using ceres::SizedCostFunction; diff --git a/quasimodo/quasimodo_models/include/core/quasimodo_point.h b/quasimodo/quasimodo_models/include/core/quasimodo_point.h new file mode 100644 index 00000000..412a4b82 --- /dev/null +++ b/quasimodo/quasimodo_models/include/core/quasimodo_point.h @@ -0,0 +1,36 @@ +#ifndef __QUASIMODO_POINT_TYPE__H +#define __QUASIMODO_POINT_TYPE__H + +#define PCL_NO_PRECOMPILE +#include +#include +#include + +struct QuasimodoPointType +{ + PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4]) + union + { + struct + { + float ce_x; + float ce_y; + float de_x; + float de_y; + }; + float data_t[4]; + }; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned +} EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment + +POINT_CLOUD_REGISTER_POINT_STRUCT (QuasimodoPointType, + (float, normal_x, normal_x) + (float, normal_y, normal_y) + (float, normal_z, normal_z) + (float, ce_x, ce_x) + (float, ce_y, ce_y) + (float, de_x, de_x) + (float, de_y, de_y) +) +#endif diff --git a/quasimodo/quasimodo_models/src/core/Camera.cpp b/quasimodo/quasimodo_models/src/core/Camera.cpp index b6b88974..9381d4ac 100644 --- a/quasimodo/quasimodo_models/src/core/Camera.cpp +++ b/quasimodo/quasimodo_models/src/core/Camera.cpp @@ -23,7 +23,7 @@ Camera::Camera(){ Camera::~Camera(){} void Camera::save(std::string path){ - printf("save camera to %s\n",path.c_str()); + printf("Camera::save(%s)\n",path.c_str()); unsigned long buffersize = 7*sizeof(double); char* buffer = new char[buffersize]; double * buffer_double = (double *)buffer; @@ -62,12 +62,13 @@ Camera * Camera::clone(){ } Camera * Camera::load(std::string path){ + printf("Camera::load(%s)\n",path.c_str()); Camera * cam = new Camera(); std::streampos size; char * buffer; - std::ifstream file (path, std::ios::in | std::ios::binary | std::ios::ate); + std::ifstream file (path+"_data.txt", std::ios::in | std::ios::binary | std::ios::ate); if (file.is_open()){ size = file.tellg(); buffer = new char [size]; diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 2d95e84d..37ef93a4 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -54,6 +54,7 @@ RGBDFrame::RGBDFrame(){ capturetime = 0; pose = Eigen::Matrix4d::Identity(); keyval = ""; + soma_id = ""; } bool updated = true; @@ -297,8 +298,8 @@ std::vector< std::vector > getImageProbs(reglib::RGBDFrame * frame, int //} RGBDFrame * RGBDFrame::clone(){ -// printf("clone()\n"); -// exit(0); + // printf("clone()\n"); + // exit(0); RGBDFrame * frame = new RGBDFrame(); frame->camera = camera->clone(); frame->rgb = rgb.clone(); @@ -329,6 +330,8 @@ RGBDFrame * RGBDFrame::clone(){ } RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals, std::string savePath, bool compute_imgedges){ + soma_id = ""; + //printf("savepath: %s\n",savePath.c_str()); bool verbose = false; if(verbose) @@ -378,7 +381,7 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt nr_labels = 1; labels = new int[nr_pixels]; - for(int i = 0; i < nr_pixels; i++){labels[i] = 0;} + for(unsigned int i = 0; i < nr_pixels; i++){labels[i] = 0;} unsigned short * depthdata = (unsigned short *)depth.data; unsigned char * rgbdata = (unsigned char *)rgb.data; @@ -388,368 +391,383 @@ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capt de.create(height,width,CV_32FC3); float * dedata = (float*)de.data; - for(int i = 0; i < 3*nr_pixels; i++){dedata[i] = 0;} + for(unsigned int i = 0; i < 3*nr_pixels; i++){dedata[i] = 0;} ce.create(height,width,CV_32FC3); float * cedata = (float*)ce.data; - for(int i = 0; i < 3*nr_pixels; i++){cedata[i] = 0;} + for(unsigned int i = 0; i < 3*nr_pixels; i++){cedata[i] = 0;} depthedges.create(height,width,CV_8UC1); unsigned char * depthedgesdata = (unsigned char *)depthedges.data; - for(int i = 0; i < nr_pixels; i++){depthedgesdata[i] = 0;} -if(compute_imgedges){ - std::vector Xvec; - int step = 1; - for(unsigned int w = step; w < width-step;w++){ - for(unsigned int h = step; h < height-step;h++){ - int ind = h*width+w; - float z = idepth*float(depthdata[ind]); - - if(w > 2 && w < width-1){ - float z0 = idepth*float(depthdata[ind-2]); - float z1 = idepth*float(depthdata[ind-1]); - float z2 = z; - float z3 = idepth*float(depthdata[ind+1]); - - float pred1 = z1 + (z1-z0); - float pred2 = z2 - (z3-z2); - - if(pred1 > 0 || pred2 > 0){ - double n0 = z0*z0; - double n1 = z1*z1; - double n2 = z2*z2; - double n3 = z3*z3; - - double meanpred = 0.5*(fabs(z2-pred1) + fabs(z1-pred2)); - double noise3 = sqrt(n1*n1+n2*n2); - float pred3 = std::min(meanpred,fabs(z1-z2)); - dedata[3*ind+1] = pred3/noise3; - Xvec.push_back(meanpred/noise3); + for(unsigned int i = 0; i < nr_pixels; i++){depthedgesdata[i] = 0;} + if(compute_imgedges){ + std::vector Xvec; + int step = 1; + for(unsigned int w = step; w < width-step;w++){ + for(unsigned int h = step; h < height-step;h++){ + int ind = h*width+w; + float z = idepth*float(depthdata[ind]); + + if(w > 2 && w < width-1){ + float z0 = idepth*float(depthdata[ind-2]); + float z1 = idepth*float(depthdata[ind-1]); + float z2 = z; + float z3 = idepth*float(depthdata[ind+1]); + + float pred1 = z1 + (z1-z0); + float pred2 = z2 - (z3-z2); + + if(pred1 > 0 || pred2 > 0){ + double n0 = z0*z0; + double n1 = z1*z1; + double n2 = z2*z2; + double n3 = z3*z3; + + double meanpred = 0.5*(fabs(z2-pred1) + fabs(z1-pred2)); + double noise3 = sqrt(n1*n1+n2*n2); + float pred3 = std::min(meanpred,fabs(z1-z2)); + dedata[3*ind+1] = pred3/noise3; + Xvec.push_back(meanpred/noise3); + } } - } - if(h > 2 && h < height-1){ - float z0 = idepth*float(depthdata[ind-2*width]); - float z1 = idepth*float(depthdata[ind-1*width]); - float z2 = z; - float z3 = idepth*float(depthdata[ind+1*width]); - - float pred1 = z1 + (z1-z0); - float pred2 = z2 - (z3-z2); - - if(pred1 > 0 || pred2 > 0){ - double n0 = z0*z0; - double n1 = z1*z1; - double n2 = z2*z2; - double n3 = z3*z3; - - double meanpred = 0.5*(fabs(z2-pred1) + fabs(z1-pred2)); - double noise3 = sqrt(n1*n1+n2*n2); - float pred3 = std::min(meanpred,fabs(z1-z2)); - dedata[3*ind+2] = pred3/noise3; - Xvec.push_back(meanpred/noise3); + if(h > 2 && h < height-1){ + float z0 = idepth*float(depthdata[ind-2*width]); + float z1 = idepth*float(depthdata[ind-1*width]); + float z2 = z; + float z3 = idepth*float(depthdata[ind+1*width]); + + float pred1 = z1 + (z1-z0); + float pred2 = z2 - (z3-z2); + + if(pred1 > 0 || pred2 > 0){ + double n0 = z0*z0; + double n1 = z1*z1; + double n2 = z2*z2; + double n3 = z3*z3; + + double meanpred = 0.5*(fabs(z2-pred1) + fabs(z1-pred2)); + double noise3 = sqrt(n1*n1+n2*n2); + float pred3 = std::min(meanpred,fabs(z1-z2)); + dedata[3*ind+2] = pred3/noise3; + Xvec.push_back(meanpred/noise3); + } } } } - } - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"depth_diff.png", 4.0*255.0*de );} + // cv::namedWindow( "de", cv::WINDOW_AUTOSIZE ); cv::imshow( "de", de); + // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); + // cv::waitKey(0); - if(verbose) - printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"depth_diff.png", 4.0*255.0*de );} - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); - for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} + if(verbose) + printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); - double stdval = 0; - for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} - stdval = sqrt(stdval/double(Xvec.size())); + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(1,Xvec.size()); + for(unsigned int i = 0; i < Xvec.size();i++){X(0,i) = Xvec[i];} - DistanceWeightFunction2PPR3 * funcZ = new DistanceWeightFunction2PPR3(); - if(savePath.size() != 0){ - funcZ->savePath = std::string(savestr)+"funcZ.m"; - } - funcZ->zeromean = true; - //funcZ->startreg = 0.001; - funcZ->startreg = 0.0001; - funcZ->debugg_print = false; - funcZ->bidir = false; - funcZ->maxp = 0.999999; - funcZ->maxd = 0.1; - funcZ->histogram_size = 100; - funcZ->fixed_histogram_size = true; - funcZ->startmaxd = funcZ->maxd; - funcZ->starthistogram_size = funcZ->histogram_size; - funcZ->blurval = 0.5; - funcZ->stdval2 = stdval; - funcZ->maxnoise = stdval; - funcZ->reset(); - funcZ->computeModel(X); - - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - if(w > 1){dedata[3*ind+1] = 1-funcZ->getProb(dedata[3*ind+1]);} - if(h > 1){dedata[3*ind+2] = 1-funcZ->getProb(dedata[3*ind+2]);} + double stdval = 0; + for(unsigned int i = 0; i < Xvec.size();i++){stdval += X(0,i)*X(0,i);} + stdval = sqrt(stdval/double(Xvec.size())); + + DistanceWeightFunction2PPR3 * funcZ = new DistanceWeightFunction2PPR3(); + if(savePath.size() != 0){ + funcZ->savePath = std::string(savestr)+"funcZ.m"; } - } + funcZ->zeromean = true; + //funcZ->startreg = 0.001; + funcZ->startreg = 0.0005; + funcZ->debugg_print = false; + funcZ->bidir = false; + funcZ->maxp = 0.999999; + funcZ->maxd = 0.1; + funcZ->histogram_size = 100; + funcZ->fixed_histogram_size = true; + funcZ->startmaxd = funcZ->maxd; + funcZ->starthistogram_size = funcZ->histogram_size; + funcZ->blurval = 0.5; + funcZ->stdval2 = stdval; + funcZ->maxnoise = stdval; + funcZ->reset(); + funcZ->computeModel(X); - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"depth_prob.png", 255.0*de );} + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + unsigned int ind = h*width+w; + if(w > 1){dedata[3*ind+1] = 1-funcZ->getProb(dedata[3*ind+1]);} + if(h > 1){dedata[3*ind+2] = 1-funcZ->getProb(dedata[3*ind+2]);} + } + } - delete funcZ; - if(verbose) - printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); + // cv::namedWindow( "de", cv::WINDOW_AUTOSIZE ); cv::imshow( "de", de); + // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); + // cv::waitKey(0); - int blursize = 5; - cv::Mat blur_rgb = rgb.clone(); - cv::GaussianBlur( blur_rgb, blur_rgb, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); + // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); + // cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); cv::imshow( "normals", normals ); + // cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", depth ); + // cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges ); + // if(stop){ cv::waitKey(0);}else{ cv::waitKey(30);} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"depth_prob.png", 255.0*de );} - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"blur_rgb.png", blur_rgb );} - unsigned char * blurdata = (unsigned char *)blur_rgb.data; + delete funcZ; - cv::Mat re; - re.create(height,width,CV_32FC3); - float * redata = (float*)re.data; - for(int i = 0; i < 3*nr_pixels; i++){redata[i] = 0;} + if(verbose) + printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); - cv::Mat ge; - ge.create(height,width,CV_32FC3); - float * gedata = (float*)ge.data; - for(int i = 0; i < 3*nr_pixels; i++){gedata[i] = 0;} + int blursize = 5; + cv::Mat blur_rgb = rgb.clone(); + cv::GaussianBlur( blur_rgb, blur_rgb, cv::Size(blursize,blursize), 0, 0, cv::BORDER_DEFAULT ); - cv::Mat be; - be.create(height,width,CV_32FC3); - float * bedata = (float*)be.data; - for(int i = 0; i < 3*nr_pixels; i++){bedata[i] = 0;} - std::vector XvecR; - std::vector XvecG; - std::vector XvecB; + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"blur_rgb.png", blur_rgb );} + unsigned char * blurdata = (unsigned char *)blur_rgb.data; - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - bedata[3*ind+1] = fabs(blurdata[3*ind+0] - blurdata[3*(ind-1)+0]); - XvecB.push_back(bedata[3*ind+1]); + cv::Mat re; + re.create(height,width,CV_32FC3); + float * redata = (float*)re.data; + for(unsigned int i = 0; i < 3*nr_pixels; i++){redata[i] = 0;} + + cv::Mat ge; + ge.create(height,width,CV_32FC3); + float * gedata = (float*)ge.data; + for(unsigned int i = 0; i < 3*nr_pixels; i++){gedata[i] = 0;} + + cv::Mat be; + be.create(height,width,CV_32FC3); + float * bedata = (float*)be.data; + for(unsigned int i = 0; i < 3*nr_pixels; i++){bedata[i] = 0;} + + std::vector XvecR; + std::vector XvecG; + std::vector XvecB; + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 0; h < height;h++){ + unsigned int ind = h*width+w; + bedata[3*ind+1] = fabs(blurdata[3*ind+0] - blurdata[3*(ind-1)+0]); + XvecB.push_back(bedata[3*ind+1]); - gedata[3*ind+1] = fabs(blurdata[3*ind+1] - blurdata[3*(ind-1)+1]); - XvecG.push_back(gedata[3*ind+1]); + gedata[3*ind+1] = fabs(blurdata[3*ind+1] - blurdata[3*(ind-1)+1]); + XvecG.push_back(gedata[3*ind+1]); - redata[3*ind+1] = fabs(blurdata[3*ind+2] - blurdata[3*(ind-1)+2]); - XvecR.push_back(redata[3*ind+1]); + redata[3*ind+1] = fabs(blurdata[3*ind+2] - blurdata[3*(ind-1)+2]); + XvecR.push_back(redata[3*ind+1]); + } } - } - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - bedata[3*ind+2] = fabs(blurdata[3*ind+0] - blurdata[3*(ind-width)+0]); - XvecB.push_back(bedata[3*ind+1]); + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 1; h < height-1;h++){ + unsigned int ind = h*width+w; + bedata[3*ind+2] = fabs(blurdata[3*ind+0] - blurdata[3*(ind-width)+0]); + XvecB.push_back(bedata[3*ind+1]); - gedata[3*ind+2] = fabs(blurdata[3*ind+1] - blurdata[3*(ind-width)+1]); - XvecG.push_back(gedata[3*ind+1]); + gedata[3*ind+2] = fabs(blurdata[3*ind+1] - blurdata[3*(ind-width)+1]); + XvecG.push_back(gedata[3*ind+1]); - redata[3*ind+2] = fabs(blurdata[3*ind+2] - blurdata[3*(ind-width)+2]); - XvecR.push_back(redata[3*ind+1]); + redata[3*ind+2] = fabs(blurdata[3*ind+2] - blurdata[3*(ind-width)+2]); + XvecR.push_back(redata[3*ind+1]); + } } - } - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_diff.png", 3.0*re );} - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_diff.png", 3.0*ge );} - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"be_diff.png", 3.0*be );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_diff.png", 3.0*re );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_diff.png", 3.0*ge );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"be_diff.png", 3.0*be );} - std::vector src_dxdata; - src_dxdata.resize(nr_pixels); - for(unsigned int i = 0; i < nr_pixels;i++){src_dxdata[i] = 0;} + std::vector src_dxdata; + src_dxdata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){src_dxdata[i] = 0;} - std::vector src_dydata; - src_dydata.resize(nr_pixels); - for(unsigned int i = 0; i < nr_pixels;i++){src_dydata[i] = 0;} + std::vector src_dydata; + src_dydata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){src_dydata[i] = 0;} - std::vector maxima_dxdata; - maxima_dxdata.resize(nr_pixels); - for(unsigned int i = 0; i < nr_pixels;i++){maxima_dxdata[i] = 0;} + std::vector maxima_dxdata; + maxima_dxdata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){maxima_dxdata[i] = 0;} - std::vector maxima_dydata; - maxima_dydata.resize(nr_pixels); - for(unsigned int i = 0; i < nr_pixels;i++){maxima_dydata[i] = 0;} + std::vector maxima_dydata; + maxima_dydata.resize(nr_pixels); + for(unsigned int i = 0; i < nr_pixels;i++){maxima_dydata[i] = 0;} - for(unsigned int w = 0; w < width;w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - src_dxdata[ind] = 0; - src_dydata[ind] = 0; + for(unsigned int w = 0; w < width;w++){ + for(unsigned int h = 0; h < height;h++){ + unsigned int ind = h*width+w; + src_dxdata[ind] = 0; + src_dydata[ind] = 0; + } } - } - unsigned int chans = 3; - for(unsigned int c = 0; c < chans;c++){ - for(unsigned int w = 1; w < width;w++){ - for(unsigned int h = 1; h < height;h++){ - int ind = h*width+w; - src_dxdata[ind] += fabs(float(blurdata[chans*ind+c] - blurdata[chans*(ind-1) +c]) / 255.0)/3.0; - src_dydata[ind] += fabs(float(blurdata[chans*ind+c] - blurdata[chans*(ind-width)+c]) / 255.0)/3.0; + unsigned int chans = 3; + for(unsigned int c = 0; c < chans;c++){ + for(unsigned int w = 1; w < width;w++){ + for(unsigned int h = 1; h < height;h++){ + int ind = h*width+w; + src_dxdata[ind] += fabs(float(blurdata[chans*ind+c] - blurdata[chans*(ind-1) +c]) / 255.0)/3.0; + src_dydata[ind] += fabs(float(blurdata[chans*ind+c] - blurdata[chans*(ind-width)+c]) / 255.0)/3.0; + } } } - } - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - maxima_dxdata[ind] = (src_dxdata[ind] > src_dxdata[ind-1] && src_dxdata[ind] >= src_dxdata[ind+1]); - maxima_dydata[ind] = (src_dydata[ind] > src_dydata[ind-width] && src_dydata[ind] >= src_dydata[ind+width]); + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + maxima_dxdata[ind] = (src_dxdata[ind] > src_dxdata[ind-1] && src_dxdata[ind] >= src_dxdata[ind+1]); + maxima_dydata[ind] = (src_dydata[ind] > src_dydata[ind-width] && src_dydata[ind] >= src_dydata[ind+width]); + } } - } - double stdvalR = 0; - for(unsigned int i = 0; i < XvecR.size();i++){stdvalR += XvecR[i]*XvecR[i];} - stdvalR = sqrt(stdvalR/double(XvecR.size())); + double stdvalR = 0; + for(unsigned int i = 0; i < XvecR.size();i++){stdvalR += XvecR[i]*XvecR[i];} + stdvalR = sqrt(stdvalR/double(XvecR.size())); - double stdvalG = 0; - for(unsigned int i = 0; i < XvecG.size();i++){stdvalG += XvecG[i]*XvecG[i];} - stdvalG = sqrt(stdvalG/double(XvecG.size())); + double stdvalG = 0; + for(unsigned int i = 0; i < XvecG.size();i++){stdvalG += XvecG[i]*XvecG[i];} + stdvalG = sqrt(stdvalG/double(XvecG.size())); - double stdvalB = 0; - for(unsigned int i = 0; i < XvecB.size();i++){stdvalB += XvecB[i]*XvecB[i];} - stdvalB = sqrt(stdvalB/double(XvecB.size())); + double stdvalB = 0; + for(unsigned int i = 0; i < XvecB.size();i++){stdvalB += XvecB[i]*XvecB[i];} + stdvalB = sqrt(stdvalB/double(XvecB.size())); - if(verbose) - printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); + if(verbose) + printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); - GeneralizedGaussianDistribution * ggdfuncR = new GeneralizedGaussianDistribution(true,false); - ggdfuncR->nr_refineiters = 1; - GeneralizedGaussianDistribution * ggdfuncG = new GeneralizedGaussianDistribution(true,false); - ggdfuncG->nr_refineiters = 1; - GeneralizedGaussianDistribution * ggdfuncB = new GeneralizedGaussianDistribution(true,false); - ggdfuncB->nr_refineiters = 1; + GeneralizedGaussianDistribution * ggdfuncR = new GeneralizedGaussianDistribution(true,false); + ggdfuncR->nr_refineiters = 1; + GeneralizedGaussianDistribution * ggdfuncG = new GeneralizedGaussianDistribution(true,false); + ggdfuncG->nr_refineiters = 1; + GeneralizedGaussianDistribution * ggdfuncB = new GeneralizedGaussianDistribution(true,false); + ggdfuncB->nr_refineiters = 1; - DistanceWeightFunction2PPR3 * funcR = new DistanceWeightFunction2PPR3(ggdfuncR); - DistanceWeightFunction2PPR3 * funcG = new DistanceWeightFunction2PPR3(ggdfuncG); - DistanceWeightFunction2PPR3 * funcB = new DistanceWeightFunction2PPR3(ggdfuncB); + DistanceWeightFunction2PPR3 * funcR = new DistanceWeightFunction2PPR3(ggdfuncR); + DistanceWeightFunction2PPR3 * funcG = new DistanceWeightFunction2PPR3(ggdfuncG); + DistanceWeightFunction2PPR3 * funcB = new DistanceWeightFunction2PPR3(ggdfuncB); - if(savePath.size() != 0){ - funcR->savePath = std::string(savestr)+"funcR.m"; - funcG->savePath = std::string(savestr)+"funcG.m"; - funcB->savePath = std::string(savestr)+"funcB.m"; - } + if(savePath.size() != 0){ + funcR->savePath = std::string(savestr)+"funcR.m"; + funcG->savePath = std::string(savestr)+"funcG.m"; + funcB->savePath = std::string(savestr)+"funcB.m"; + } - funcR->zeromean = funcG->zeromean = funcB->zeromean = true; - funcR->maxp = funcG->maxp = funcB->maxp = 0.999999999999999999; - funcR->startreg = funcG->startreg = funcB->startreg = 0.001; - funcR->debugg_print = funcG->debugg_print = funcB->debugg_print = false; - funcR->maxd = funcG->maxd = funcB->maxd = 256.0; - funcR->histogram_size = funcG->histogram_size = funcB->histogram_size = 256; - funcR->fixed_histogram_size = funcG->fixed_histogram_size = funcB->fixed_histogram_size = true; - funcR->startmaxd = funcG->startmaxd = funcB->startmaxd = funcR->maxd; - funcR->starthistogram_size = funcG->starthistogram_size = funcB->starthistogram_size = funcR->histogram_size; - //funcR->blurval = funcG->blurval = funcB->blurval = 0.005; - funcR->blurval = funcG->blurval = funcB->blurval = 0.5; - - funcR->stdval2 = stdvalR; - funcR->maxnoise = stdvalR; - funcG->stdval2 = stdvalG; - funcG->maxnoise = stdvalG; - funcB->stdval2 = stdvalB; - funcB->maxnoise = stdvalB; - - funcR->reset(); - ((DistanceWeightFunction2*)funcR)->computeModel(XvecR); - funcG->reset(); - ((DistanceWeightFunction2*)funcG)->computeModel(XvecG); - funcB->reset(); - ((DistanceWeightFunction2*)funcB)->computeModel(XvecB); - - for(unsigned int w = 1; w < width-1;w++){ - for(unsigned int h = 1; h < height-1;h++){ - int ind = h*width+w; - if(w > 1){ - double pr = funcR->getProb(redata[3*ind+1]); - double pg = funcG->getProb(gedata[3*ind+1]); - double pb = funcB->getProb(bedata[3*ind+1]); - double pc = pr*pg*pb/(pr*pg*pb+(1-pr)*(1-pg)*(1-pb)); - redata[3*ind+1] = 1-pr; - gedata[3*ind+1] = 1-pg; - bedata[3*ind+1] = 1-pb; - cedata[3*ind+1] = 1-pc; - } + funcR->zeromean = funcG->zeromean = funcB->zeromean = true; + funcR->maxp = funcG->maxp = funcB->maxp = 0.999999999999999999; + funcR->startreg = funcG->startreg = funcB->startreg = 0.5; + funcR->debugg_print = funcG->debugg_print = funcB->debugg_print = false; + funcR->maxd = funcG->maxd = funcB->maxd = 256.0; + funcR->histogram_size = funcG->histogram_size = funcB->histogram_size = 256; + funcR->fixed_histogram_size = funcG->fixed_histogram_size = funcB->fixed_histogram_size = true; + funcR->startmaxd = funcG->startmaxd = funcB->startmaxd = funcR->maxd; + funcR->starthistogram_size = funcG->starthistogram_size = funcB->starthistogram_size = funcR->histogram_size; + //funcR->blurval = funcG->blurval = funcB->blurval = 0.005; + funcR->blurval = funcG->blurval = funcB->blurval = 0.5; + + funcR->stdval2 = stdvalR; + funcR->maxnoise = stdvalR; + funcG->stdval2 = stdvalG; + funcG->maxnoise = stdvalG; + funcB->stdval2 = stdvalB; + funcB->maxnoise = stdvalB; + + funcR->reset(); + ((DistanceWeightFunction2*)funcR)->computeModel(XvecR); + funcG->reset(); + ((DistanceWeightFunction2*)funcG)->computeModel(XvecG); + funcB->reset(); + ((DistanceWeightFunction2*)funcB)->computeModel(XvecB); + + for(unsigned int w = 1; w < width-1;w++){ + for(unsigned int h = 1; h < height-1;h++){ + int ind = h*width+w; + if(w > 1){ + double pr = funcR->getProb(redata[3*ind+1]); + double pg = funcG->getProb(gedata[3*ind+1]); + double pb = funcB->getProb(bedata[3*ind+1]); + double pc = pr*pg*pb/(pr*pg*pb+(1-pr)*(1-pg)*(1-pb)); + redata[3*ind+1] = 1-pr; + gedata[3*ind+1] = 1-pg; + bedata[3*ind+1] = 1-pb; + cedata[3*ind+1] = 1-pc; + } - if(h > 1){ - double pr = funcR->getProb(redata[3*ind+2]); - double pg = funcG->getProb(gedata[3*ind+2]); - double pb = funcB->getProb(bedata[3*ind+2]); - double pc = pr*pg*pb/(pr*pg*pb+(1-pr)*(1-pg)*(1-pb)); - redata[3*ind+2] = 1-pr; - gedata[3*ind+2] = 1-pg; - bedata[3*ind+2] = 1-pb; - cedata[3*ind+2] = 1-pc; + if(h > 1){ + double pr = funcR->getProb(redata[3*ind+2]); + double pg = funcG->getProb(gedata[3*ind+2]); + double pb = funcB->getProb(bedata[3*ind+2]); + double pc = pr*pg*pb/(pr*pg*pb+(1-pr)*(1-pg)*(1-pb)); + redata[3*ind+2] = 1-pr; + gedata[3*ind+2] = 1-pg; + bedata[3*ind+2] = 1-pb; + cedata[3*ind+2] = 1-pc; + } } } - } - delete funcR; - delete funcG; - delete funcB; + delete funcR; + delete funcG; + delete funcB; - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_prob.png", 127.0*re );} - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_prob.png", 127.0*ge );} - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"be_prob.png", 127.0*be );} - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ce_prob.png", 127.0*ce );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"re_prob.png", 127.0*re );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ge_prob.png", 127.0*ge );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"be_prob.png", 127.0*be );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ce_prob.png", 127.0*ce );} - if(verbose) - printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); + if(verbose) + printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); - cv::Mat det; - det.create(height,width,CV_8UC1); - unsigned char * detdata = (unsigned char*)det.data; - for(int i = 0; i < nr_pixels; i++){ - detdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5)); - } + cv::Mat det; + det.create(height,width,CV_8UC1); + unsigned char * detdata = (unsigned char*)det.data; + for(unsigned int i = 0; i < nr_pixels; i++){ + detdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5)); + } - cv::Mat cenms; - cenms.create(height,width,CV_32FC3); - float * cenmsdata = (float*)cenms.data; - for(int i = 0; i < nr_pixels; i++){ - double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]); - edgep = std::max(0.000000000000000000001,edgep); - cenmsdata[3*i+0] = edgep; - cenmsdata[3*i+1] = edgep; - cenmsdata[3*i+2] = edgep; - } + cv::Mat cenms; + cenms.create(height,width,CV_32FC3); + float * cenmsdata = (float*)cenms.data; + for(unsigned int i = 0; i < nr_pixels; i++){ + double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]); + edgep = std::max(0.000000000000000000001,edgep); + cenmsdata[3*i+0] = edgep; + cenmsdata[3*i+1] = edgep; + cenmsdata[3*i+2] = edgep; + } - if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ce_non_maxima_supress_prob.png", 255.0*cenms );} + if(savePath.size() != 0){cv::imwrite( std::string(savestr)+"ce_non_maxima_supress_prob.png", 255.0*cenms );} -// for(int i = 0; i < nr_pixels; i++){ -// double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]); -// edgep = std::max(0.00001,edgep); -// cenmsdata[3*i+0] = 0; -// cenmsdata[3*i+1] = std::max(0.00001,cenmsdata[3*i+1]); -// cenmsdata[3*i+2] = std::max(0.00001,edgep); -// } + // for(int i = 0; i < nr_pixels; i++){ + // double edgep = std::max(cedata[3*i+1]*maxima_dxdata[i],cedata[3*i+2]*maxima_dydata[i]); + // edgep = std::max(0.00001,edgep); + // cenmsdata[3*i+0] = 0; + // cenmsdata[3*i+1] = std::max(0.00001,cenmsdata[3*i+1]); + // cenmsdata[3*i+2] = std::max(0.00001,edgep); + // } - // cv::namedWindow( "colour", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour", ce); - // cv::namedWindow( "colour+nms", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour+nms", cenms); - // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); - // cv::waitKey(0); + // cv::namedWindow( "colour", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour", ce); + // cv::namedWindow( "colour+nms", cv::WINDOW_AUTOSIZE ); cv::imshow( "colour+nms", cenms); + // cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb); + // cv::waitKey(0); - ce = cenms; + ce = cenms; - int dilation_size = 2; - cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) ); - unsigned char * det_dilatedata = (unsigned char*)det_dilate.data; + int dilation_size = 2; + cv::dilate( det, det_dilate, getStructuringElement( cv::MORPH_RECT, cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), cv::Point( dilation_size, dilation_size ) ) ); + unsigned char * det_dilatedata = (unsigned char*)det_dilate.data; -// depthedges.create(height,width,CV_8UC1); -// unsigned char * depthedgesdata = (unsigned char *)depthedges.data; - for(int i = 0; i < nr_pixels; i++){ - //depthedgesdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5) || (cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5)); - depthedgesdata[i] = 255*(((cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5)) && (det_dilatedata[i] == 0)); + // depthedges.create(height,width,CV_8UC1); + // unsigned char * depthedgesdata = (unsigned char *)depthedges.data; + for(unsigned int i = 0; i < nr_pixels; i++){ + //depthedgesdata[i] = 255*((dedata[3*i+1] > 0.5) || (dedata[3*i+2] > 0.5) || (cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5)); + depthedgesdata[i] = 255*(((cedata[3*i+1]*maxima_dxdata[i] > 0.5) || (cedata[3*i+2]*maxima_dydata[i] > 0.5)) && (det_dilatedata[i] == 0)); + } } -} // cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges); // cv::waitKey(0); @@ -765,9 +783,9 @@ if(compute_imgedges){ cloud->height = height; cloud->points.resize(nr_pixels); - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + unsigned int ind = h*width+w; pcl::PointXYZRGBA & p = cloud->points[ind]; p.b = rgbdata[3*ind+0]; p.g = rgbdata[3*ind+1]; @@ -797,9 +815,9 @@ if(compute_imgedges){ ne.setDepthDependentSmoothing (depth_dependent_smoothing); ne.compute(*normals_cloud); - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + unsigned int ind = h*width+w; pcl::Normal p2 = normals_cloud->points[ind]; if(!isnan(p2.normal_x)){ normalsdata[3*ind+0] = p2.normal_x; @@ -818,9 +836,9 @@ if(compute_imgedges){ out.create(height,width,CV_8UC3); unsigned char * outdata = out.data; - for(int w = 0; w < width; w++){ - for(int h = 0; h < height;h++){ - int ind = h*width+w; + for(unsigned int w = 0; w < width; w++){ + for(unsigned int h = 0; h < height;h++){ + unsigned int ind = h*width+w; pcl::Normal p2 = normals_cloud->points[ind]; if(!isnan(p2.normal_x)){ outdata[3*ind+0] = fabs(255.0*p2.normal_x); @@ -1013,26 +1031,83 @@ void RGBDFrame::savePCD(std::string path, Eigen::Matrix4d pose){ int success = pcl::io::savePCDFileBinaryCompressed(path,*cloud); } +std::string type2str(int type) { + std::string r; + + uchar depth = type & CV_MAT_DEPTH_MASK; + uchar chans = 1 + (type >> CV_CN_SHIFT); + + switch ( depth ) { + case CV_8U: r = "8U"; break; + case CV_8S: r = "8S"; break; + case CV_16U: r = "16U"; break; + case CV_16S: r = "16S"; break; + case CV_32S: r = "32S"; break; + case CV_32F: r = "32F"; break; + case CV_64F: r = "64F"; break; + default: r = "User"; break; + } + + r += "C"; + r += (chans+'0'); + + return r; +} + +#include +#include +#include + + void RGBDFrame::save(std::string path){ //printf("saving frame %i to %s\n",id,path.c_str()); cv::imwrite( path+"_rgb.png", rgb ); cv::imwrite( path+"_depth.png", depth ); + cv::imwrite( path+"_det_dilate.png", det_dilate ); + cv::imwrite( path+"_depthedges.png", depthedges ); + + const unsigned int width = camera->width; + const unsigned int height = camera->height; + + std::ofstream normalsoutfile (path+"_normalsdata.txt",std::ofstream::binary); + normalsoutfile.write ((char*)(normals.data),width*height*3*sizeof(float)); + normalsoutfile.close(); + + std::ofstream ceoutfile (path+"_cedata.txt",std::ofstream::binary); + ceoutfile.write ((char*)(ce.data),width*height*3*sizeof(float)); + ceoutfile.close(); + + std::ofstream deoutfile (path+"_dedata.txt",std::ofstream::binary); + deoutfile.write ((char*)(de.data),width*height*3*sizeof(float)); + deoutfile.close(); - unsigned long buffersize = 19*sizeof(double); + unsigned long buffersize = 22*sizeof(double)+keyval.length()+soma_id.length(); char* buffer = new char[buffersize]; double * buffer_double = (double *)buffer; unsigned long * buffer_long = (unsigned long *)buffer; int counter = 0; - buffer_double[counter++] = capturetime; + buffer_double[counter++] = capturetime; // CaptureTime for(int i = 0; i < 4; i++){ for(int j = 0; j < 4; j++){ buffer_double[counter++] = pose(i,j); } + } // Pose + buffer_long[counter++] = sweepid; // Sweepid + buffer_long[counter++] = camera->id; // Cameraid + buffer_long[counter++] = id; // id + buffer_long[counter++] = keyval.length(); // keyval.length() + buffer_long[counter++] = soma_id.length(); // soma_id.length() + + unsigned int count4 = sizeof(double)*counter; + for(unsigned int i = 0; i < keyval.length();i++){ + buffer[count4++] = keyval[i]; } - buffer_long[counter++] = sweepid; - buffer_long[counter++] = camera->id; + for(unsigned int i = 0; i < soma_id.length();i++){ + buffer[count4++] = soma_id[i]; + } + std::ofstream outfile (path+"_data.txt",std::ofstream::binary); outfile.write (buffer,buffersize); outfile.close(); @@ -1040,7 +1115,10 @@ void RGBDFrame::save(std::string path){ } RGBDFrame * RGBDFrame::load(Camera * cam, std::string path){ - //printf("RGBDFrame * RGBDFrame::load(Camera * cam, std::string path)\n"); + double startTime = getTime(); + + const unsigned int width = cam->width; + const unsigned int height = cam->height; std::streampos size; char * buffer; @@ -1067,14 +1145,69 @@ RGBDFrame * RGBDFrame::load(Camera * cam, std::string path){ } int sweepid = buffer_long[counter++]; int camera_id = buffer_long[counter++]; + int id = buffer_long[counter++]; + int keyvallength = buffer_long[counter++]; + int soma_idlength = buffer_long[counter++]; + + + std::string keyval; + keyval.resize(keyvallength); + std::string soma_id; + soma_id.resize(soma_idlength); + + unsigned int count4 = sizeof(double)*counter; + for(unsigned int i = 0; i < keyvallength;i++){ + keyval[i] = buffer[count4++]; + } + + for(unsigned int i = 0; i < soma_idlength;i++){ + soma_id[i] = buffer[count4++]; + } cv::Mat rgb = cv::imread(path+"_rgb.png", -1); // Read the file cv::Mat depth = cv::imread(path+"_depth.png", -1); // Read the file - - RGBDFrame * frame = new RGBDFrame(cam,rgb,depth,capturetime,pose); + cv::Mat det_dilate = cv::imread(path+"_det_dilate.png", -1); // Read the file + cv::Mat depthedges = cv::imread(path+"_depthedges.png", -1); // Read the file + + RGBDFrame * frame = new RGBDFrame(); + frame->rgb = rgb; + frame->depth = depth; + frame->det_dilate = det_dilate; + frame->depthedges = depthedges; + frame->camera = cam; + frame->capturetime = capturetime; frame->sweepid = sweepid; - //printf("sweepid: %i",sweepid); + frame->pose = pose; + + frame->normals.create(height,width,CV_32FC3); + frame->ce.create(height,width,CV_32FC3); + frame->de.create(height,width,CV_32FC3); + + std::ifstream normalsfile (path+"_normalsdata.txt", std::ios::in | std::ios::binary | std::ios::ate); + if (normalsfile.is_open()){ + size = normalsfile.tellg(); + normalsfile.seekg (0, std::ios::beg); + normalsfile.read ((char*)(frame->normals.data), size); + normalsfile.close(); + } + + std::ifstream cefile (path+"_cedata.txt", std::ios::in | std::ios::binary | std::ios::ate); + if (cefile.is_open()){ + size = cefile.tellg(); + cefile.seekg (0, std::ios::beg); + cefile.read ((char*)(frame->ce.data), size); + cefile.close(); + } + + std::ifstream defile (path+"_dedata.txt", std::ios::in | std::ios::binary | std::ios::ate); + if (defile.is_open()){ + size = defile.tellg(); + defile.seekg (0, std::ios::beg); + defile.read ((char*)(frame->de.data), size); + defile.close(); + } +//printf("%s::%i time: %5.5fs\n",__PRETTY_FUNCTION__,__LINE__,getTime()-startTime); startTime = getTime(); return frame; }else{printf("cant open %s\n",(path+"/data.txt").c_str());} } @@ -1228,7 +1361,7 @@ std::vector< std::vector > RGBDFrame::getImageProbs(bool depthonly){ for(unsigned int i = 0; i < nr_pixels;i++){ //dxc[i] = 1.0-std::max(dedata[3*i+1],cedata[3*i+1]); //dyc[i] = 1.0-std::max(dedata[3*i+2],cedata[3*i+2]); -//ProcessedImages + //ProcessedImages @@ -1247,11 +1380,11 @@ std::vector< std::vector > RGBDFrame::getImageProbs(bool depthonly){ } -// cv::namedWindow( "Colour edges" , cv::WINDOW_AUTOSIZE ); cv::imshow( "Colour edges", ce); -// cv::namedWindow( "Depth edges" , cv::WINDOW_AUTOSIZE ); cv::imshow( "Depth edges", de); -// cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); -// cv::namedWindow( "det_dilate" , cv::WINDOW_AUTOSIZE ); cv::imshow( "det_dilate", det_dilate ); -// cv::waitKey(0); + // cv::namedWindow( "Colour edges" , cv::WINDOW_AUTOSIZE ); cv::imshow( "Colour edges", ce); + // cv::namedWindow( "Depth edges" , cv::WINDOW_AUTOSIZE ); cv::imshow( "Depth edges", de); + // cv::namedWindow( "rgb" , cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); + // cv::namedWindow( "det_dilate" , cv::WINDOW_AUTOSIZE ); cv::imshow( "det_dilate", det_dilate ); + // cv::waitKey(0); std::vector< std::vector > probs2; probs2.push_back(dxc); @@ -1293,7 +1426,7 @@ void RGBDFrame::saveCombinedProcessedImages(std::string path){ for(unsigned long h = 0; h < height;h++){ for(unsigned long w = 0; w < width;w++){ - if(normalsdata[3*(h*width+w)+0] <= 1){ + if(normalsdata[3*(h*width+w)+0] <= 1){ combineddata[3*(h*(3*width)+w+0*width)+0] = 255.0*fabs(normalsdata[3*(h*width+w)+0]); combineddata[3*(h*(3*width)+w+0*width)+1] = 255.0*fabs(normalsdata[3*(h*width+w)+1]); combineddata[3*(h*(3*width)+w+0*width)+2] = 255.0*fabs(normalsdata[3*(h*width+w)+2]); @@ -1313,9 +1446,9 @@ void RGBDFrame::saveCombinedProcessedImages(std::string path){ cv::imwrite( path, combined ); -// cv::namedWindow( "combined" , cv::WINDOW_AUTOSIZE ); -// cv::imshow( "combined", combined ); -// cv::waitKey(0); + // cv::namedWindow( "combined" , cv::WINDOW_AUTOSIZE ); + // cv::imshow( "combined", combined ); + // cv::waitKey(0); } } From 0edf6efd952ea01ac4f8e9c4352eb7cf18ab9fe3 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 22 Nov 2016 23:31:59 +0100 Subject: [PATCH 224/255] ... --- .../metaroom_additional_view_processing.cpp | 65 +++++++++++-------- 1 file changed, 38 insertions(+), 27 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index fe79cb7c..635428e1 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -520,12 +520,12 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: unsigned char * maskdata = (unsigned char *)fullmask.data; for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} + std::vector metaroom_frames; for (size_t i=0; i= counter){ image_geometry::PinholeCameraModel aCameraModel = roomData.vIntermediateRoomCloudCamParamsCorrected[i]; - //image_geometry::PinholeCameraModel aCameraModel = roomData.vIntermediateRoomCloudCamParams[i]; reglib::Camera * cam = new reglib::Camera(); cam->fx = aCameraModel.fx(); cam->fy = aCameraModel.fy(); @@ -544,7 +544,6 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: metaroom_frames.push_back(frames[i]); } - metaroom_frames.back()->camera->print(); if(sweepmodel == 0){ sweepmodel = new reglib::Model(metaroom_frames.back(),fullmask); @@ -568,35 +567,52 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: std::vector viewdepths; std::vector viewtfs; + unsigned int current_counter = metaroom_frames.size(); + + std::vector av_frames; QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); for (auto objectFile : objectFiles){ auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); for (unsigned int i=0; iheight,cloud->width,CV_8UC3); + unsigned char * rgbdata = (unsigned char *)rgb.data; - CloudPtr cloud = object.vAdditionalViews[i]; + cv::Mat depth; + depth.create(cloud->height,cloud->width,CV_16UC1); + unsigned short * depthdata = (unsigned short *)depth.data; + + unsigned int nr_data = cloud->height * cloud->width; + for(unsigned int j = 0; j < nr_data; j++){ + PointType p = cloud->points[j]; + rgbdata[3*j+0] = p.b; + rgbdata[3*j+1] = p.g; + rgbdata[3*j+2] = p.r; + depthdata[j] = short(5000.0 * p.z); + } - cv::Mat rgb; - rgb.create(cloud->height,cloud->width,CV_8UC3); - unsigned char * rgbdata = (unsigned char *)rgb.data; + geometry_msgs::TransformStamped msg; + tf::transformStampedTFToMsg(object.vAdditionalViewsTransforms[i], msg); + long sec = msg.header.stamp.sec; + long nsec = msg.header.stamp.nsec; + double time = double(sec)+1e-9*double(nsec); - cv::Mat depth; - depth.create(cloud->height,cloud->width,CV_16UC1); - unsigned short * depthdata = (unsigned short *)depth.data; + Eigen::Matrix4d m = quasimodo_brain::getMat(object.vAdditionalViewsTransforms[i]); - unsigned int nr_data = cloud->height * cloud->width; - for(unsigned int j = 0; j < nr_data; j++){ - PointType p = cloud->points[j]; - rgbdata[3*j+0] = p.b; - rgbdata[3*j+1] = p.g; - rgbdata[3*j+2] = p.r; - depthdata[j] = short(5000.0 * p.z); - } + reglib::Camera * cam = metaroom_frames.front()->camera->clone(); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth,time, m,true,savePath);//a.matrix()); - viewrgbs.push_back(rgb); - viewdepths.push_back(depth); - viewtfs.push_back(object.vAdditionalViewsTransforms[i]); + sprintf(buf,"%s/frame_%5.5i",sweep_folder.c_str(),current_counter); + frame->save(std::string(buf)); + cam->save(std::string(buf)+"_camera"); + av_frames.push_back(frame); + } + current_counter++; } } @@ -619,13 +635,8 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: // std::vector times; // for(unsigned int i = 0; i < 3000 && i < viewrgbs.size(); i++){ // printf("additional view: %i\n",i); -// geometry_msgs::TransformStamped msg; -// tf::transformStampedTFToMsg(viewtfs[i], msg); -// long sec = msg.header.stamp.sec; -// long nsec = msg.header.stamp.nsec; -// double time = double(sec)+1e-9*double(nsec); -// Eigen::Matrix4d m = quasimodo_brain::getMat(viewtfs[i]); +// // cout << m << endl << endl; From d4ec82a968c2e8573a4cd7b2a1eec1951ab01506 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 22 Nov 2016 23:32:37 +0100 Subject: [PATCH 225/255] ... --- .../quasimodo_brain/src/metaroom_additional_view_processing.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 635428e1..661638bf 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -570,7 +570,6 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: unsigned int current_counter = metaroom_frames.size(); std::vector av_frames; - QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); for (auto objectFile : objectFiles){ auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); for (unsigned int i=0; i Date: Wed, 23 Nov 2016 13:45:10 +0100 Subject: [PATCH 226/255] Added script for removing all models --- .../scripts/remove_all_models.py | 53 +++++++++++++++++++ 1 file changed, 53 insertions(+) create mode 100644 quasimodo/quasimodo_object_search/scripts/remove_all_models.py diff --git a/quasimodo/quasimodo_object_search/scripts/remove_all_models.py b/quasimodo/quasimodo_object_search/scripts/remove_all_models.py new file mode 100644 index 00000000..70801ce3 --- /dev/null +++ b/quasimodo/quasimodo_object_search/scripts/remove_all_models.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python + +import roslib +import rospy +from sensor_msgs.msg import PointCloud2, PointField +#from world_modeling.srv import * +from cv_bridge import CvBridge, CvBridgeError +import cv2 + +# SOMA2 stuff +from soma_msgs.msg import SOMAObject +from mongodb_store.message_store import MessageStoreProxy +from soma_manager.srv import * +from geometry_msgs.msg import Pose, Transform +from observation_registration_services.srv import ProcessRegisteredViews, ProcessRegisteredViewsRequest, ProcessRegisteredViewsResponse +from soma_io.state import World, Object +from datetime import datetime, timedelta +from quasimodo_msgs.msg import fused_world_state_object, image_array +from quasimodo_msgs.srv import transform_cloud, transform_cloudRequest, transform_cloudResponse +from quasimodo_msgs.srv import index_cloud, index_cloudRequest, index_cloudResponse +from quasimodo_msgs.srv import mask_pointclouds, mask_pointcloudsRequest, mask_pointcloudsResponse +from quasimodo_msgs.srv import insert_model, insert_modelRequest, insert_modelResponse +from geometry_msgs.msg import Pose +from std_msgs.msg import Empty +import time +import json + +pub = () + +def remove_model_cb(req): + + msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') + # msg_store.delete(req.object_id) + # resp = insert_modelResponse() + # resp.object_id = req.object_id + + results = msg_store.query(type=fused_world_state_object) + + for message, metadata in results: + message.removed_at = "manually removed" + msg_store.update_id(metadata['id'], new_obj) + + empty_msg = Empty() + pub.publish(empty_msg) + + return resp + +if __name__ == '__main__': + rospy.init_node('remove_all_models', anonymous = False) + + pub = rospy.Publisher("/model/added_to_db", data_class=Empty, queue_size=None) + + rospy.spin() From 992083ffa0e0204c32e9e38278b4a8b4667372fe Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Wed, 23 Nov 2016 13:46:34 +0100 Subject: [PATCH 227/255] Added script for removing all models --- .../quasimodo_object_search/scripts/remove_all_models.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_object_search/scripts/remove_all_models.py b/quasimodo/quasimodo_object_search/scripts/remove_all_models.py index 70801ce3..87a60a77 100644 --- a/quasimodo/quasimodo_object_search/scripts/remove_all_models.py +++ b/quasimodo/quasimodo_object_search/scripts/remove_all_models.py @@ -37,8 +37,9 @@ def remove_model_cb(req): results = msg_store.query(type=fused_world_state_object) for message, metadata in results: - message.removed_at = "manually removed" - msg_store.update_id(metadata['id'], new_obj) + if len(message.removed_at) == 0: + message.removed_at = "manually removed" + msg_store.update_id(metadata['id'], new_obj) empty_msg = Empty() pub.publish(empty_msg) From 7730613514ac0383288ee7a1a66087aa28e41410 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Wed, 23 Nov 2016 13:51:14 +0100 Subject: [PATCH 228/255] Added script for removing all models --- .../quasimodo_object_search/scripts/remove_all_models.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) mode change 100644 => 100755 quasimodo/quasimodo_object_search/scripts/remove_all_models.py diff --git a/quasimodo/quasimodo_object_search/scripts/remove_all_models.py b/quasimodo/quasimodo_object_search/scripts/remove_all_models.py old mode 100644 new mode 100755 index 87a60a77..4c73144c --- a/quasimodo/quasimodo_object_search/scripts/remove_all_models.py +++ b/quasimodo/quasimodo_object_search/scripts/remove_all_models.py @@ -27,7 +27,7 @@ pub = () -def remove_model_cb(req): +def remove_models(): msg_store = MessageStoreProxy(database='world_state', collection='quasimodo') # msg_store.delete(req.object_id) @@ -51,4 +51,4 @@ def remove_model_cb(req): pub = rospy.Publisher("/model/added_to_db", data_class=Empty, queue_size=None) - rospy.spin() + remove_models() From 10a8da6686c3b8758e5e29088307962e844d6a85 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Wed, 23 Nov 2016 13:59:11 +0100 Subject: [PATCH 229/255] Working --- .../quasimodo_object_search/scripts/remove_all_models.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/quasimodo/quasimodo_object_search/scripts/remove_all_models.py b/quasimodo/quasimodo_object_search/scripts/remove_all_models.py index 4c73144c..00aacc48 100755 --- a/quasimodo/quasimodo_object_search/scripts/remove_all_models.py +++ b/quasimodo/quasimodo_object_search/scripts/remove_all_models.py @@ -34,17 +34,16 @@ def remove_models(): # resp = insert_modelResponse() # resp.object_id = req.object_id - results = msg_store.query(type=fused_world_state_object) + results = msg_store.query(type='quasimodo_msgs/fused_world_state_object') for message, metadata in results: if len(message.removed_at) == 0: message.removed_at = "manually removed" - msg_store.update_id(metadata['id'], new_obj) + msg_store.update_id(metadata['_id'], message) empty_msg = Empty() pub.publish(empty_msg) - return resp if __name__ == '__main__': rospy.init_node('remove_all_models', anonymous = False) From 9f70bfa9ec36b7b71775864dd4bc78fa55ef4aa9 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Wed, 23 Nov 2016 16:19:20 +0100 Subject: [PATCH 230/255] Fast commit! GOTTA RUN! --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 8 +- .../metaroom_additional_view_processing.cpp | 219 ++++++++---------- .../src/registration/MassRegistrationPPR2.cpp | 21 +- 3 files changed, 118 insertions(+), 130 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index c1978c8f..c5e08a46 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -130,12 +130,12 @@ std::vector readPoseXML(std::string xmlFile){ QFile file(xmlFile.c_str()); if (!file.exists()){ - ROS_ERROR("Could not open file %s to load poses.",xmlFile.c_str()); + //ROS_ERROR("Could not open file %s to load poses.",xmlFile.c_str()); return poses; } file.open(QIODevice::ReadOnly); - ROS_INFO_STREAM("Parsing xml file: "< readPoseXML(std::string xmlFile){ token = xmlReader->readNext();//Pose elementName = xmlReader->name().toString(); - std::cout << pose << std::endl << std::endl; + //std::cout << pose << std::endl << std::endl; poses.push_back(pose); } } } delete xmlReader; - printf("done readPoseXML\n"); + //printf("done readPoseXML\n"); return poses; } diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 661638bf..aca8a1d6 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -493,8 +493,6 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: reglib::Camera * cam = reglib::Camera::load(std::string(buf)+"_camera"); if(cam == 0){break;} - cam->print(); - reglib::RGBDFrame * frame = reglib::RGBDFrame::load(cam,std::string(buf)); if(frame == 0){delete cam; break;} frames.push_back(frame); @@ -560,20 +558,21 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: sweepmodel->recomputeModelPoints(); quasimodo_brain::saveSuperPoints(sweep_folder+"/sweepmodel_superpoints.bin",sweepmodel->points,Eigen::Matrix4d::Identity(),1.0); } - printf("sweepmodel fully loaded!"); + printf("sweepmodel fully loaded!\n"); //Load rest of model if possible - std::vector viewrgbs; - std::vector viewdepths; - std::vector viewtfs; + std::vector both_unrefined; + both_unrefined.push_back(Eigen::Matrix4d::Identity()); unsigned int current_counter = metaroom_frames.size(); std::vector av_frames; + std::vector av_mm; for (auto objectFile : objectFiles){ auto object = loadDynamicObjectFromSingleSweep(sweep_folder+objectFile.toStdString(),false); for (unsigned int i=0; icamera->clone(); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth,time, m,true,savePath);//a.matrix()); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb,depth,time, m,true,saveVisuals_sp);//a.matrix()); sprintf(buf,"%s/frame_%5.5i",sweep_folder.c_str(),current_counter); frame->save(std::string(buf)); cam->save(std::string(buf)+"_camera"); av_frames.push_back(frame); + frames.push_back(frame); } + both_unrefined.push_back(metaroom_frames.front()->pose.inverse()*av_frames.back()->pose); + av_mm.push_back(new reglib::ModelMask(fullmask)); + //av_frames.back()->show(true); current_counter++; } } + reglib::Model * fullmodel = new reglib::Model(); + fullmodel->savePath = saveVisuals_sp+"/"; + fullmodel->frames = sweepmodel->frames; + fullmodel->relativeposes = sweepmodel->relativeposes; + fullmodel->modelmasks = sweepmodel->modelmasks; + fullmodel->points = sweepmodel->points; + for(unsigned int i = 0; i < av_frames.size(); i++){ + fullmodel->frames.push_back(av_frames[i]); + fullmodel->modelmasks.push_back(av_mm[i]); + } -// for(unsigned int i = 0; (i < sweep->frames.size()) && (sweep->frames.size() == sweepPoses.size()) ; i++){ -// sweep->frames[i]->pose = sweep->frames.front()->pose * sweepPoses[i]; -// sweep->relativeposes[i] = sweepPoses[i]; -// if(basecam != 0){ -// delete sweep->frames[i]->camera; -// sweep->frames[i]->camera = basecam->clone(); -// } -// } + std::vector loadedPoses = quasimodo_brain::readPoseXML(sweep_folder+"/fullmodel_poses.xml"); + std::vector spvec = quasimodo_brain::getSuperPoints(sweep_folder+"/fullmodel_superpoints.bin"); -// std::vector frames; -// std::vector masks; -// std::vector unrefined; + //Check if poses already computed + if(av_frames.size() > 0){ + if(loadedPoses.size() != (av_frames.size()+metaroom_frames.size())){ + printf("TIME TO RECOMPUTE\n"); -// std::vector both_unrefined; -// both_unrefined.push_back(Eigen::Matrix4d::Identity()); -// std::vector times; -// for(unsigned int i = 0; i < 3000 && i < viewrgbs.size(); i++){ -// printf("additional view: %i\n",i); + reglib::RegistrationRefinement * refinement = new reglib::RegistrationRefinement(); + refinement->viewer = viewer; + refinement->visualizationLvl = visualization_lvl_regini; + refinement->normalize_matchweights = false; + refinement->target_points = 4000; -// + reglib::CloudData * cd1 = sweepmodel->getCD(sweepmodel->points.size()); + refinement->setDst(cd1); -// cout << m << endl << endl; - -// unrefined.push_back(m); -// times.push_back(time); - -// cv::Mat fullmask; -// fullmask.create(480,640,CV_8UC1); -// unsigned char * maskdata = (unsigned char *)fullmask.data; -// for(int j = 0; j < 480*640; j++){maskdata[j] = 255;} -// masks.push_back(new reglib::ModelMask(fullmask)); - -// reglib::Camera * cam = sweep->frames.front()->camera->clone(); -// reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,viewrgbs[i],viewdepths[i],time, m,true,savePath);//a.matrix()); -// frames.push_back(frame); - -// both_unrefined.push_back(sweep->frames.front()->pose.inverse()*m); -// } - -// reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); -// reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( sweep, reg); -// mu->occlusion_penalty = 15; -// mu->massreg_timeout = 60*4; -// mu->viewer = viewer; - -// sweep->recomputeModelPoints(); -// reglib::Model * fullmodel = new reglib::Model(); -// fullmodel->savePath = savePath+"/"; -// fullmodel->frames = sweep->frames; -// fullmodel->relativeposes = sweep->relativeposes; -// fullmodel->modelmasks = sweep->modelmasks; -// //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); -// if(frames.size() > 0){ - -// reglib::RegistrationRefinement * refinement = new reglib::RegistrationRefinement(); -// refinement->viewer = viewer; -// refinement->visualizationLvl = visualization_lvl_regini; -// refinement->normalize_matchweights = false; -// refinement->target_points = 4000; -// ////double register_setup_start = getTime(); - -// //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); -// reglib::CloudData * cd1 = sweep->getCD(sweep->points.size()); -// refinement->setDst(cd1); - -// fullmodel->points = frames.front()->getSuperPoints(Eigen::Matrix4d::Identity(),1,false); -// reglib::CloudData * cd2 = fullmodel->getCD(fullmodel->points.size()); -// refinement->setSrc(cd2); -// //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); -// //////printf("register_setup_start: %5.5f\n",getTime()-register_setup_start); -// ////double register_compute_start = getTime(); -// Eigen::Matrix4d guess = both_unrefined[1]*both_unrefined[0].inverse(); - -// reglib::FusionResults fr = refinement->getTransform(guess); -// Eigen::Matrix4d offset = fr.guess*guess.inverse(); -// for(unsigned int i = 1; i < both_unrefined.size(); i++){ -// both_unrefined[i] = offset*both_unrefined[i]; -// } + fullmodel->points = av_frames.front()->getSuperPoints(Eigen::Matrix4d::Identity(),1,false); + reglib::CloudData * cd2 = fullmodel->getCD(fullmodel->points.size()); + refinement->setSrc(cd2); -// delete refinement; + Eigen::Matrix4d guess = both_unrefined[1]*both_unrefined[0].inverse(); -// fullmodel->points.clear(); -// delete cd1; -// delete cd2; + reglib::FusionResults fr = refinement->getTransform(guess); + Eigen::Matrix4d offset = fr.guess*guess.inverse(); + for(unsigned int i = 1; i < both_unrefined.size(); i++){ + both_unrefined[i] = offset*both_unrefined[i]; + } -// reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.25); -// if(savePath.size() != 0){ -// bgmassreg->savePath = savePath+"/processAV_"+std::to_string(fullmodel->id); -// } -// //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); - -// bgmassreg->timeout = 300; -// bgmassreg->viewer = viewer; -// bgmassreg->use_surface = true; -// bgmassreg->use_depthedge = false; -// bgmassreg->visualizationLvl = visualization_lvl_regref; -// bgmassreg->maskstep = 5; -// bgmassreg->nomaskstep = 5; -// bgmassreg->nomask = true; -// bgmassreg->stopval = 0.0005; -// bgmassreg->addModel(sweep); -// bgmassreg->setData(frames,masks); -// // exit(0); -// //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); - -// reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); - -// delete bgmassreg; - -// for(unsigned int i = 0; i < frames.size(); i++){frames[i]->pose = sweep->frames.front()->pose * bgmfr.poses[i+1];} - -// for(unsigned int i = 0; i < frames.size(); i++){ -// fullmodel->frames.push_back(frames[i]); -// fullmodel->modelmasks.push_back(masks[i]); -// fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); -// } -// fullmodel->recomputeModelPoints(); + delete refinement; + + fullmodel->points.clear(); + delete cd1; + delete cd2; + + reglib::MassRegistrationPPR2 * bgmassreg = new reglib::MassRegistrationPPR2(0.25); + if(saveVisuals_sp.size() != 0){bgmassreg->savePath = saveVisuals_sp+"/processAV_"+std::to_string(fullmodel->id);} + bgmassreg->timeout = 300; + bgmassreg->viewer = viewer; + bgmassreg->use_surface = true; + bgmassreg->use_depthedge = false; + bgmassreg->visualizationLvl = visualization_lvl_regref; + bgmassreg->maskstep = 5; + bgmassreg->nomaskstep = 5; + bgmassreg->nomask = true; + bgmassreg->stopval = 0.0005; + bgmassreg->addModel(sweepmodel); + bgmassreg->setData(av_frames,av_mm); + reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); + delete bgmassreg; + + for(unsigned int i = 0; i < av_frames.size(); i++){ + av_frames[i]->pose = sweepmodel->frames.front()->pose * bgmfr.poses[i+1]; + fullmodel->relativeposes.push_back(bgmfr.poses[i+1]); + } + + quasimodo_brain::savePoses(sweep_folder+"/fullmodel_poses.xml", fullmodel->relativeposes); +//relativeposes[i], frames[i], modelmasks[i] + }else{ + fullmodel->relativeposes = loadedPoses; + } + + + fullmodel->points = sweepmodel->points; + if(loadedPoses.size() != (av_frames.size()+metaroom_frames.size())){//If poses changed, recompute superpoints + for(unsigned int i = 0; i < av_frames.size(); i++){ + fullmodel->addSuperPoints( + fullmodel->points, + fullmodel->relativeposes[i+metaroom_frames.size()], + fullmodel->frames[i+metaroom_frames.size()], + fullmodel->modelmasks[i+metaroom_frames.size()]); + } + } + quasimodo_brain::saveSuperPoints(sweep_folder+"/fullmodel_superpoints.bin",fullmodel->points,Eigen::Matrix4d::Identity(),1.0); // if(savePath.size() != 0){ // pcl::PointCloud::Ptr cld = fullmodel->getPCLnormalcloud(1, false); // pcl::io::savePCDFileBinaryCompressed (savePath+"/processAV_"+std::to_string(fullmodel->id)+"_fused.pcd", *cld); // } // //pcl::PointCloud::Ptr Model::getPCLnormalcloud(int step, bool color){ +// + }else{ + quasimodo_brain::savePoses(sweep_folder+"/fullmodel_poses.txt", fullmodel->relativeposes); + quasimodo_brain::saveSuperPoints(sweep_folder+"/fullmodel_superpoints.bin",fullmodel->points,Eigen::Matrix4d::Identity(),1.0); + } -// }else{ -// fullmodel->points = sweep->points; -// } + printf("fullmodel->points.size() = %i\n",fullmodel->points.size()); // delete reg; // delete mu; @@ -751,7 +724,7 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: //Load the unloaded frames... exit(0); - +/* reglib::Model * fullmodel; if(viewgroup_nrviews == (additional_nrviews+metaroom_nrviews) && !recomputeRelativePoses){ printf("time to read old\n"); @@ -775,7 +748,7 @@ exit(0); quasimodo_brain::writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); } //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); - +*/ return fullmodel; } diff --git a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp index 5cecabdf..bcfca359 100644 --- a/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp +++ b/quasimodo/quasimodo_models/src/registration/MassRegistrationPPR2.cpp @@ -25,7 +25,7 @@ MassRegistrationPPR2::MassRegistrationPPR2(double startreg, bool visualize){ DistanceWeightFunction2PPR2 * dwf = new DistanceWeightFunction2PPR2(); dwf->update_size = true; dwf->startreg = startreg; - dwf->debugg_print = true; + dwf->debugg_print = false; func = dwf; @@ -147,7 +147,6 @@ void MassRegistrationPPR2::addModel(Model * model){ is_ok.push_back(false); - long count = model->points.size()/step; long i = points.size()-1; if(count < 10){ @@ -183,8 +182,24 @@ void MassRegistrationPPR2::addModel(Model * model){ Eigen::Matrix & tXn = transformed_normals[i]; Eigen::VectorXd information (count); + std::vector pind; + pind.resize(model->points.size()); + for(unsigned long c = 0; c < model->points.size(); c++){ + pind[c] = c; + } + + for(unsigned long c = 0; c < count; c++){ + long rind = rand() % model->points.size(); + long pind1 = pind[c]; + pind[c] = pind[rind]; + pind[rind] = pind1; + } + + + for(unsigned long c = 0; c < count; c++){ - superpoint & sp = model->points[c*step]; + //superpoint & sp = model->points[c*step]; + superpoint & sp = model->points[pind[c]]; ap[3*c+0] = sp.point(0); ap[3*c+1] = sp.point(1); ap[3*c+2] = sp.point(2); From 6bcafeeada81a4f24cabe3d36e3583fef1f05c03 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 24 Nov 2016 10:02:11 +0100 Subject: [PATCH 231/255] cleaned in sweep reg --- strands_sweep_registration/include/Camera.h | 41 ----------- strands_sweep_registration/include/Frame.h | 39 ----------- .../include/PixelFunction.h | 20 ------ .../include/ProblemFrameConnection.h | 41 ----------- .../include/RobotContainer.h | 68 ------------------- strands_sweep_registration/include/Sweep.h | 25 ------- .../include/camera_parameters.h | 57 ---------------- .../include/pair3DError.h | 41 ----------- strands_sweep_registration/include/util.h | 16 ----- strands_sweep_registration/src/Camera.cpp | 2 +- strands_sweep_registration/src/Frame.cpp | 2 +- .../src/PixelFunction.cpp | 2 +- .../src/ProblemFrameConnection.cpp | 2 +- .../src/RobotContainer.cpp | 6 +- strands_sweep_registration/src/Sweep.cpp | 2 +- .../src/pair3DError.cpp | 4 +- strands_sweep_registration/src/util.cpp | 2 +- 17 files changed, 11 insertions(+), 359 deletions(-) delete mode 100644 strands_sweep_registration/include/Camera.h delete mode 100644 strands_sweep_registration/include/Frame.h delete mode 100644 strands_sweep_registration/include/PixelFunction.h delete mode 100644 strands_sweep_registration/include/ProblemFrameConnection.h delete mode 100644 strands_sweep_registration/include/RobotContainer.h delete mode 100644 strands_sweep_registration/include/Sweep.h delete mode 100644 strands_sweep_registration/include/camera_parameters.h delete mode 100644 strands_sweep_registration/include/pair3DError.h delete mode 100644 strands_sweep_registration/include/util.h diff --git a/strands_sweep_registration/include/Camera.h b/strands_sweep_registration/include/Camera.h deleted file mode 100644 index 702561a4..00000000 --- a/strands_sweep_registration/include/Camera.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef Camera_H_ -#define Camera_H_ - -#include -#include -#include - -#include "PixelFunction.h" - -class Camera { - public: - float fx; - float fy; - float cx; - float cy; - - int width; - int height; - - int version; - - PixelFunction ** pixelsFunctions; - - unsigned int coefs_degree; - unsigned int coefs_width; - unsigned int coefs_height; - double * multiplierCoeffs; - - Camera( float fx_, float fy_, float cx_, float cy_, int width_, int height_); - virtual ~Camera(); - - virtual double * getCoeffs(unsigned int w, unsigned int h); - virtual double getGridMultiplier(unsigned int w, unsigned int h, double z); - virtual double getMultiplier(double w, double h, double z); - virtual void save(std::string path); - virtual void load(std::string path); - virtual void show(); - virtual void print(); -}; - -#endif diff --git a/strands_sweep_registration/include/Frame.h b/strands_sweep_registration/include/Frame.h deleted file mode 100644 index 1bff8978..00000000 --- a/strands_sweep_registration/include/Frame.h +++ /dev/null @@ -1,39 +0,0 @@ -#ifndef Frame_H_ -#define Frame_H_ - -#include - -#include "Camera.h" - -#include "opencv2/core/core.hpp" -#include "opencv2/highgui/highgui.hpp" -#include "opencv/cv.hpp" -#include "opencv/highgui.h" - -#include - -class Camera; - -class Frame -{ - public: - Camera * camera; - - int id; - int framepos; - - int featuretype; - std::vector keypoints; - cv::Mat descriptors; - std::vector keypoint_depth; - - std::vector keypoint_location; - - Frame(Camera * camera_); - Frame(Camera * camera_, float * rgb_data_, float * depth_data_); - Frame(Camera * camera_, std::vector k, std::vector depth, cv::Mat d); // initialize from ORB features driectly - ~Frame(); - - void recalculateFullPoints(); -}; -#endif diff --git a/strands_sweep_registration/include/PixelFunction.h b/strands_sweep_registration/include/PixelFunction.h deleted file mode 100644 index 74c55dba..00000000 --- a/strands_sweep_registration/include/PixelFunction.h +++ /dev/null @@ -1,20 +0,0 @@ -#ifndef PixelFunction_H_ -#define PixelFunction_H_ -#include - -class PixelFunction -{ - public: - float r_mul; - float g_mul; - float b_mul; - float d_mul; - PixelFunction(); - ~PixelFunction(); - - void getValues(float * pixel); - void update(std::vector data, float bias = 2, float random = 0, float step = 1); - void addOutput( std::vector & pixeldata_char, std::vector & pixeldata_counter); - void load(char * buffer,int & pos); -}; -#endif diff --git a/strands_sweep_registration/include/ProblemFrameConnection.h b/strands_sweep_registration/include/ProblemFrameConnection.h deleted file mode 100644 index 77302b1b..00000000 --- a/strands_sweep_registration/include/ProblemFrameConnection.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef ProblemFrameConnection_H_ -#define ProblemFrameConnection_H_ - -#include "pair3DError.h" -#include "Frame.h" - -class ProblemFrameConnection { - public: - Frame * src; - Frame * dst; - - double * params; - double * src_variable; - double * dst_variable; - - std::vector< Eigen::Vector3f > full_src_points; - std::vector< Eigen::Vector3f > full_dst_points; - std::vector< float > full_noisemul; - - - std::vector< Eigen::Vector3f > src_points; - std::vector< Eigen::Vector3f > dst_points; - std::vector< float > noisemul; - - std::vector< float > possible_matches_fdistance; - std::vector< float > possible_matches_edistance; - std::vector src_possible_matches_id; - std::vector dst_possible_matches_id; - - std::vector src_matches; - std::vector dst_matches; - - ProblemFrameConnection(ceres::Problem & problem, Frame * src_, Frame * dst_, double * shared_params, double * src_variable_, double * dst_variable_, float weight = 1, bool show = false); - void addMatchesToProblem(ceres::Problem & problem, std::vector< CostFunction * > & costfunctions, double hubersize = 0.1); - void addMatchesToProblem(ceres::Problem & problem, float weight = 1); - void findPossibleMatches(float di = 1, float pi = 0, Eigen::Matrix4f pose = Eigen::Matrix4f::Identity()); - void recalculatePoints(); - ~ProblemFrameConnection(); -}; - -#endif diff --git a/strands_sweep_registration/include/RobotContainer.h b/strands_sweep_registration/include/RobotContainer.h deleted file mode 100644 index 63690178..00000000 --- a/strands_sweep_registration/include/RobotContainer.h +++ /dev/null @@ -1,68 +0,0 @@ -#ifndef RobotContainer_H_ -#define RobotContainer_H_ - -#include -#include "Frame.h" -#include "Camera.h" -#include "Sweep.h" - -#include "pair3DError.h" -#include "ProblemFrameConnection.h" - -#include -#include -#include - -class RobotContainer -{ - public: - - unsigned int width; - unsigned int height; - - unsigned int gx; - unsigned int todox; - unsigned int gy; - unsigned int todoy; - unsigned int ** inds; - - float * rgb; - float * depth; - - double * shared_params; - - double *** poses; - - Camera * camera; - std::vector sweeps; - std::vector alignedSweep; - - - std::vector paths; - - bool viewer_initialized; - boost::shared_ptr viewer; - - RobotContainer(unsigned int gx_,unsigned int todox_,unsigned int gy_,unsigned int todoy_); - ~RobotContainer(); - - void initializeCamera(double fx, double fy, double cx, double cy, unsigned int w, unsigned int h); - void addToTraining(std::string path); // this loads a whole sweep and extracts ORB features - bool addToTrainingORBFeatures(std::string path); // this loads previously comptued ORB features - - std::vector runInitialTraining(); - void refineTraining(); - std::vector train(); - bool isCalibrated(); - - std::vector alignAndStoreSweeps(); - - void saveAllSweeps(std::string path); - - void show(); - -private: - void saveSweep(Sweep*, std::string path); - -}; -#endif diff --git a/strands_sweep_registration/include/Sweep.h b/strands_sweep_registration/include/Sweep.h deleted file mode 100644 index 9cc8c5b3..00000000 --- a/strands_sweep_registration/include/Sweep.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef Sweep_H_ -#define Sweep_H_ -#include -#include -#include "Frame.h" - -class Sweep -{ - public: - std::string idtag; - std::string xmlpath; - - int width; - int height; - Frame *** frames; - Eigen::Matrix4f ** poses; - - Sweep(int width_, int height_, std::vector framesvec); - Sweep(); - virtual ~Sweep(); - - Eigen::Matrix4f align(Sweep * sweep, float threshold = 0.02, int ransac_iter = 20000, int nr_points = 3); - std::vector getPoseVector(); -}; -#endif diff --git a/strands_sweep_registration/include/camera_parameters.h b/strands_sweep_registration/include/camera_parameters.h deleted file mode 100644 index cdc5f1cf..00000000 --- a/strands_sweep_registration/include/camera_parameters.h +++ /dev/null @@ -1,57 +0,0 @@ -#ifndef __CAMERA_PARAMETERS__ -#define __CAMERA_PARAMETERS__ - -#include - -class CameraParameters -{ - public: - static const CameraParameters & get(double fx = 0.0, double fy = 0.0, double cx = 0.0, double cy = 0.0, unsigned int width = 0, unsigned int height = 0) - { - static const CameraParameters instance(fx, fy, cx, cy, width, height); - return instance; - } - const double & fx() const - { - return fx_; - } - const double & fy() const - { - return fy_; - } - const double & cx() const - { - return cx_; - } - const double & cy() const - { - return cy_; - } - const unsigned int & width() const - { - return width_; - } - const unsigned int & height() const - { - return height_; - } - private: - CameraParameters(double fx, double fy, double cx, double cy, unsigned int width, unsigned int height) - : fx_(fx), - fy_(fy), - cx_(cx), - cy_(cy), - width_(width), - height_(height) - { - assert(fx_ > 0.0 && fy_ > 0.0 && cx_ > 0.0 && cy_ > 0.0 && height_ > 0 && width_ > 0 && "Please set the camera parameters!"); - } - const double fx_; - const double fy_; - const double cx_; - const double cy_; - const unsigned int width_; - const unsigned int height_; -}; - -#endif diff --git a/strands_sweep_registration/include/pair3DError.h b/strands_sweep_registration/include/pair3DError.h deleted file mode 100644 index 238ce2cd..00000000 --- a/strands_sweep_registration/include/pair3DError.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef pair3DError_H_ -#define pair3DError_H_ -#include "ceres/ceres.h" -#include "gflags/gflags.h" -#include "glog/logging.h" -#include "ceres/rotation.h" -#include "ceres/iteration_callback.h" - -#include "util.h" - - - -using ceres::NumericDiffCostFunction; -using ceres::SizedCostFunction; -using ceres::CENTRAL; -using ceres::AutoDiffCostFunction; -using ceres::CostFunction; -using ceres::Problem; -using ceres::Solver; -using ceres::Solve; -using ceres::Solve; - -class pair3DError : public SizedCostFunction<3, 6, 6, 4> { - public: - static bool optimizeCameraParams; - pair3DError(double sw, double sh, double sz,double dw, double dh, double dz, double weight); - ~pair3DError(); - bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const ; - - int id; - double sw; - double sh; - double sz; - double dw; - double dh; - double dz; - double weight; - //bool optimizeCameraParams; - double information; -}; -#endif diff --git a/strands_sweep_registration/include/util.h b/strands_sweep_registration/include/util.h deleted file mode 100644 index 496a80ed..00000000 --- a/strands_sweep_registration/include/util.h +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef myutil_H_ -#define myutil_H_ - -// PCL specific includes -#include -#include -#include -#include -#include - -int popcount_lauradoux(uint64_t *buf, uint32_t size); -template Eigen::Matrix getMat(const T* const camera, int mode = 0); -template void transformPoint(const T* const camera, T * point, int mode = 0); -void getMat(const double* const camera, double * mat); - -#endif diff --git a/strands_sweep_registration/src/Camera.cpp b/strands_sweep_registration/src/Camera.cpp index f6c3a533..d80e140e 100644 --- a/strands_sweep_registration/src/Camera.cpp +++ b/strands_sweep_registration/src/Camera.cpp @@ -1,4 +1,4 @@ -#include "strands_sweep_registration/Camera.h" +#include "../include/strands_sweep_registration/Camera.h" #include #include "opencv2/highgui/highgui.hpp" #include "opencv/cv.hpp" diff --git a/strands_sweep_registration/src/Frame.cpp b/strands_sweep_registration/src/Frame.cpp index 02a776ae..125e73d2 100644 --- a/strands_sweep_registration/src/Frame.cpp +++ b/strands_sweep_registration/src/Frame.cpp @@ -1,4 +1,4 @@ -#include "strands_sweep_registration/Frame.h" +#include "../include/strands_sweep_registration/Frame.h" #include #include #include diff --git a/strands_sweep_registration/src/PixelFunction.cpp b/strands_sweep_registration/src/PixelFunction.cpp index 83f4c09a..b04a17c6 100644 --- a/strands_sweep_registration/src/PixelFunction.cpp +++ b/strands_sweep_registration/src/PixelFunction.cpp @@ -1,4 +1,4 @@ -#include "strands_sweep_registration/PixelFunction.h" +#include "../include/strands_sweep_registration/PixelFunction.h" #include #include #include diff --git a/strands_sweep_registration/src/ProblemFrameConnection.cpp b/strands_sweep_registration/src/ProblemFrameConnection.cpp index 7be63e01..759be5f2 100644 --- a/strands_sweep_registration/src/ProblemFrameConnection.cpp +++ b/strands_sweep_registration/src/ProblemFrameConnection.cpp @@ -1,4 +1,4 @@ -#include "strands_sweep_registration/ProblemFrameConnection.h" +#include "../include/strands_sweep_registration/ProblemFrameConnection.h" ProblemFrameConnection::ProblemFrameConnection(ceres::Problem & problem, Frame * src_, Frame * dst_, double * shared_params, double * src_variable_, double * dst_variable_, float weight, bool show){ src = src_; diff --git a/strands_sweep_registration/src/RobotContainer.cpp b/strands_sweep_registration/src/RobotContainer.cpp index f89a192e..9d5290f5 100644 --- a/strands_sweep_registration/src/RobotContainer.cpp +++ b/strands_sweep_registration/src/RobotContainer.cpp @@ -1,10 +1,10 @@ #include #include -#include "strands_sweep_registration/pair3DError.h" -#include "strands_sweep_registration/RobotContainer.h" +#include "../include/strands_sweep_registration/pair3DError.h" +#include "../include/strands_sweep_registration/RobotContainer.h" -#include "strands_sweep_registration/camera_parameters.h" +#include "../include/strands_sweep_registration/camera_parameters.h" typedef pcl::PointXYZRGB PointType; typedef typename SimpleSummaryParser::EntityStruct Entities; diff --git a/strands_sweep_registration/src/Sweep.cpp b/strands_sweep_registration/src/Sweep.cpp index a4133820..4ab75bf0 100644 --- a/strands_sweep_registration/src/Sweep.cpp +++ b/strands_sweep_registration/src/Sweep.cpp @@ -1,4 +1,4 @@ -#include "strands_sweep_registration/Sweep.h" +#include "../include/strands_sweep_registration/Sweep.h" #include "cv.h" #include "highgui.h" diff --git a/strands_sweep_registration/src/pair3DError.cpp b/strands_sweep_registration/src/pair3DError.cpp index 05d26e32..87510dba 100644 --- a/strands_sweep_registration/src/pair3DError.cpp +++ b/strands_sweep_registration/src/pair3DError.cpp @@ -1,5 +1,5 @@ -#include "strands_sweep_registration/pair3DError.h" -#include "strands_sweep_registration/camera_parameters.h" +#include "../include/strands_sweep_registration/pair3DError.h" +#include "../include/strands_sweep_registration/camera_parameters.h" int sumid = 0; diff --git a/strands_sweep_registration/src/util.cpp b/strands_sweep_registration/src/util.cpp index 08a42cfa..c8b13f14 100644 --- a/strands_sweep_registration/src/util.cpp +++ b/strands_sweep_registration/src/util.cpp @@ -1,4 +1,4 @@ -#include "strands_sweep_registration/util.h" +#include "../include/strands_sweep_registration/util.h" int popcount_lauradoux(uint64_t *buf, uint32_t size) { const uint64_t* data = (uint64_t*) buf; From 12478393b8f87257c1b4fd7dcc8698c258b5ef37 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 24 Nov 2016 10:02:30 +0100 Subject: [PATCH 232/255] updated fastload of data --- .../quasimodo_brain/src/metaroom_additional_view_processing.cpp | 1 - quasimodo/quasimodo_models/src/core/RGBDFrame.cpp | 2 ++ 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index aca8a1d6..88a83ec2 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -723,7 +723,6 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: //Load the unloaded frames... -exit(0); /* reglib::Model * fullmodel; if(viewgroup_nrviews == (additional_nrviews+metaroom_nrviews) && !recomputeRelativePoses){ diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 37ef93a4..2982af08 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -55,6 +55,7 @@ RGBDFrame::RGBDFrame(){ pose = Eigen::Matrix4d::Identity(); keyval = ""; soma_id = ""; + labels = 0; } bool updated = true; @@ -331,6 +332,7 @@ RGBDFrame * RGBDFrame::clone(){ RGBDFrame::RGBDFrame(Camera * camera_, cv::Mat rgb_, cv::Mat depth_, double capturetime_, Eigen::Matrix4d pose_, bool compute_normals, std::string savePath, bool compute_imgedges){ soma_id = ""; + labels = 0; //printf("savepath: %s\n",savePath.c_str()); bool verbose = false; From 9464ab164a6adb6d6232a58e5f695713b9229d79 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 24 Nov 2016 11:38:26 +0100 Subject: [PATCH 233/255] ... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 4 +- .../quasimodo_models/src/core/Camera.cpp | 4 +- .../src/modelupdater/ModelUpdater.cpp | 72 ++++++++++--------- 3 files changed, 44 insertions(+), 36 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index c5e08a46..99ed05b9 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -756,8 +756,8 @@ void segment(std::vector< reglib::Model * > bgs, std::vector< reglib::Model * > massregmod->timeout = 1200; massregmod->viewer = viewer; massregmod->visualizationLvl = debugg > 1; - massregmod->maskstep = 4;//std::max(1,int(0.4*double(models[i]->frames.size()))); - massregmod->nomaskstep = 4;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); + massregmod->maskstep = 6;//std::max(1,int(0.4*double(models[i]->frames.size()))); + massregmod->nomaskstep = 6;//std::max(3,int(0.5+0.*double(models[i]->frames.size())));//std::max(1,int(0.5+1.0*double(model->frames.size()))); massregmod->nomask = true; massregmod->stopval = 0.0001; diff --git a/quasimodo/quasimodo_models/src/core/Camera.cpp b/quasimodo/quasimodo_models/src/core/Camera.cpp index 9381d4ac..b83aaaa0 100644 --- a/quasimodo/quasimodo_models/src/core/Camera.cpp +++ b/quasimodo/quasimodo_models/src/core/Camera.cpp @@ -23,7 +23,7 @@ Camera::Camera(){ Camera::~Camera(){} void Camera::save(std::string path){ - printf("Camera::save(%s)\n",path.c_str()); + //printf("Camera::save(%s)\n",path.c_str()); unsigned long buffersize = 7*sizeof(double); char* buffer = new char[buffersize]; double * buffer_double = (double *)buffer; @@ -62,7 +62,7 @@ Camera * Camera::clone(){ } Camera * Camera::load(std::string path){ - printf("Camera::load(%s)\n",path.c_str()); + //printf("Camera::load(%s)\n",path.c_str()); Camera * cam = new Camera(); std::streampos size; diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp index c02ef199..bdba461e 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdater.cpp @@ -2447,7 +2447,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s dfunc = dfuncTMP; dfuncTMP->startreg = 0.00; dfuncTMP->max_under_mean = false; - dfuncTMP->debugg_print = true; + dfuncTMP->debugg_print = false; dfuncTMP->bidir = true; dfuncTMP->zeromean = false; dfuncTMP->maxp = 0.9999; @@ -2937,6 +2937,13 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s double score0 = 0; double score1 = 0; + + double pscore0 = 0; + double pscore1 = 0; + + double nscore0 = 0; + double nscore1 = 0; + double totsum = 0; while(todocounter < todo.size()){ unsigned long cind = todo[todocounter++]; @@ -2957,11 +2964,16 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s else{ s0 += p0 - p2;} score0 += s0; + if(s0 > 0){ pscore0 += s0;} + else{ nscore0 += s0;} + double s1 = 0; if(p0 > p2){s1 += p1 - p0;} else{ s1 += p1 - p2;} score1 += s1; + if(s1 > 0){ pscore1 += s1;} + else{ nscore1 += s1;} totsum++; } @@ -3003,8 +3015,22 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } + score0 = 100.0*pscore0+nscore0; + score1 = pscore1+nscore1; + + labelID.push_back(0); - if(debugg != 0){printf("score0: %f score1: %f\n",score0,score1);} + if(debugg != 0){ + if(totsum > 100){ + printf("---------------------------\n"); + printf("score0: %10.10f score1: %10.10f ",score0,score1); + printf("totsum: %10.10f\n",totsum); + + printf("pscore0: %10.10f nscore0: %10.10f ",pscore0,nscore0); + printf("pscore1: %10.10f nscore1: %10.10f\n",pscore1,nscore1); + } + } + if(std::max(score0,score1) < 100){continue;} if(score1 > score0){ @@ -3014,11 +3040,13 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s sr.scores_dynamic.push_back(score1); sr.total_dynamic.push_back(totsum); }else{ - labelID.back() = --nr_obj_mov; - if(debugg != 0){printf("Moving: %f -> %f\n",score0,totsum);} - sr.component_moving.push_back(todo); - sr.scores_moving.push_back(score0); - sr.total_moving.push_back(totsum); + if(score0 > 10000){ + labelID.back() = --nr_obj_mov; + if(debugg != 0){printf("Moving: %f -> %f\n",score0,totsum);} + sr.component_moving.push_back(todo); + sr.scores_moving.push_back(score0); + sr.total_moving.push_back(totsum); + } } } } @@ -3088,10 +3116,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_priors.pcd", *cloud_sample); -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); -// viewer->spin(); - for(unsigned int i = 0; i < current_point; i++){ cloud_sample->points[i].r = 0; cloud_sample->points[i].g = 0; @@ -3114,9 +3138,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_classes.pcd", *cloud_sample); -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); -// viewer->spin(); for(unsigned int i = 0; i < current_point; i++){ cloud_sample->points[i].r = 0; @@ -3128,11 +3149,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s int randr = rand()%256; int randg = rand()%256; int randb = rand()%256; -// if(randr*randr + randg*randg + (255-randr)*(255-randr) < 1000){ -// randr = rand()%256; -// randg = rand()%256; -// randb = rand()%256; -// } + for(unsigned int i = 0; i < sr.component_dynamic[c].size(); i++){ cloud_sample->points[sr.component_dynamic[c][i]].r = randr; cloud_sample->points[sr.component_dynamic[c][i]].g = randg; @@ -3144,11 +3161,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s int randr = rand()%256; int randg = rand()%256; int randb = rand()%256; -// if(randr*randr + randg*randg + (255-randr)*(255-randr) < 1000){ -// randr = rand()%256; -// randg = rand()%256; -// randb = rand()%256; -// } + for(unsigned int i = 0; i < sr.component_moving[c].size(); i++){ cloud_sample->points[sr.component_moving[c][i]].r = randr; cloud_sample->points[sr.component_moving[c][i]].g = randg; @@ -3156,9 +3169,6 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s } } pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_clusters.pcd", *cloud_sample); -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); -// viewer->spin(); cloud->width = cloud->points.size(); cloud->height = 1; @@ -3173,12 +3183,10 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s cloud_sample->width = cloud_sample->points.size(); cloud_sample->height = 1; pcl::io::savePCDFileBinaryCompressed (savePath+"/segment_"+std::to_string(segment_run_counter)+"_dynamicobjects.pcd", *cloud_sample); -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud_sample, pcl::visualization::PointCloudColorHandlerRGBField(cloud_sample), "cloud"); -// viewer->spin(); - } + + printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime); if(debugg){ pcl::PointCloud::Ptr cloud_sample (new pcl::PointCloud); @@ -3266,7 +3274,7 @@ void ModelUpdater::computeMovingDynamicStatic(std::vector & movemask, s delete[] valids; delete[] priors; delete[] prior_weights; - printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime); + //printf("computeMovingDynamicStatic total time: %5.5fs\n",getTime()-computeMovingDynamicStatic_startTime); } /* From d6b4b3c113cdb9f1629acf5fac24304aa4fe1e6e Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 24 Nov 2016 16:08:55 +0100 Subject: [PATCH 234/255] removed soma2 etc --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 93 ++++++++++++++ quasimodo/quasimodo_brain/src/Util/Util.h | 8 ++ .../metaroom_additional_view_processing.cpp | 105 ++++------------ quasimodo/quasimodo_brain/src/voPCD.cpp | 4 +- .../include/core/quasimodo_point.h | 30 +++-- .../quasimodo_models/src/core/RGBDFrame.cpp | 116 +++++++++++------- 6 files changed, 222 insertions(+), 134 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 99ed05b9..ec3f46e1 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -2,6 +2,99 @@ namespace quasimodo_brain { +soma_llsd_msgs::Scene getScene(ros::NodeHandle & n, reglib::RGBDFrame * frame, std::string current_waypointid, std::string roomRunNumber){ + soma_llsd_msgs::Scene sc; + if(frame->soma_id.length() > 0){//Frame already connected to scene, get from db + ros::ServiceClient client = n.serviceClient("/soma_llsd/get_scene"); + ROS_INFO("Waiting for /soma_llsd/get_scene service..."); + if (!client.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/get_scene service!"); + return sc; + } + ROS_INFO("Got /soma_llsd/get_scene service"); + + soma_llsd::GetScene srv; + srv.request.scene_id = frame->soma_id; + if (client.call(srv)) { + ROS_INFO("Got /soma_llsd/get_scene"); + return srv.response.response; + } + } + + geometry_msgs::Pose pose; + tf::poseEigenToMsg (Eigen::Affine3d(frame->pose), pose); + + cv_bridge::CvImage rgbBridgeImage; + rgbBridgeImage.image = frame->rgb; + rgbBridgeImage.encoding = "bgr8"; + + cv_bridge::CvImage depthBridgeImage; + depthBridgeImage.image = frame->depth; + depthBridgeImage.encoding = "mono16"; + + sensor_msgs::PointCloud2 input; + pcl::toROSMsg (*frame->getPCLcloud(),input);//, *transformed_cloud); + input.header.frame_id = "/map"; + + ros::ServiceClient insertclient = n.serviceClient("/soma_llsd/insert_scene"); + ROS_INFO("Waiting for /soma_llsd/insert_scene service..."); + if (!insertclient.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/insert_scene service!"); + return sc; + } + ROS_INFO("Got /soma_llsd/insert_scene service"); + + soma_llsd::InsertScene scene; + scene.request.rgb_img = *(rgbBridgeImage.toImageMsg()); + scene.request.depth_img = *(depthBridgeImage.toImageMsg()); + scene.request.camera_info.K[0] = frame->camera->fx; + scene.request.camera_info.K[4] = frame->camera->fy; + scene.request.camera_info.K[2] = frame->camera->cx; + scene.request.camera_info.K[5] = frame->camera->cy; + scene.request.robot_pose = pose; + scene.request.cloud = input; + scene.request.waypoint = current_waypointid; + scene.request.episode_id = roomRunNumber; + + if (!insertclient.call(scene)) { + ROS_ERROR("Failed to call service /soma_llsd/insert_scene"); + return sc; + } + return scene.response.response; +} + +reglib::Camera * getCam(sensor_msgs::CameraInfo & info){ + reglib::Camera * cam = new reglib::Camera(); + if(info.K[0] > 0){ + cam->fx = info.K[0]; + cam->fy = info.K[4]; + cam->cx = info.K[2]; + cam->cy = info.K[5]; + } + return cam; +} + +reglib::RGBDFrame * getFrame(soma_llsd_msgs::Scene & scene){ + + reglib::Camera * cam = getCam(scene.camera_info); + + cv_bridge::CvImagePtr rgb_ptr; + try{ rgb_ptr = cv_bridge::toCvCopy(scene.rgb_img, sensor_msgs::image_encodings::BGR8);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat rgb = rgb_ptr->image; + + cv_bridge::CvImagePtr depth_ptr; + try{ depth_ptr = cv_bridge::toCvCopy(scene.depth_img, sensor_msgs::image_encodings::MONO16);} + catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + cv::Mat depth = depth_ptr->image; + + Eigen::Affine3d epose; + tf::poseMsgToEigen(scene.robot_pose, epose); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, 0, epose.matrix(),true,"",true); + frame->soma_id = scene.id; + return frame; +} + std::vector getSuperPoints(std::string path){ std::vector spvec; diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index 552d4d22..f9ba4a57 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -37,8 +37,16 @@ #include #include +#include +#include + namespace quasimodo_brain { +soma_llsd_msgs::Scene getScene(ros::NodeHandle& n, reglib::RGBDFrame * frame, std::string current_waypointid = "", std::string roomRunNumber = ""); + +reglib::Camera * getCam(sensor_msgs::CameraInfo & info); +reglib::RGBDFrame * getFrame(soma_llsd_msgs::Scene & scene); + std::vector getSuperPoints(std::string path); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 88a83ec2..4f3c89e5 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -2142,6 +2142,8 @@ bool testPath(std::string path){ return false; } + + void addSceneToLastMetaroom(std::string soma_id){ printf("void addSceneToLastMetaroom(%s)\n",soma_id.c_str()); vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); @@ -2156,7 +2158,7 @@ void addSceneToLastMetaroom(std::string soma_id){ ROS_INFO("Waiting for /soma_llsd/get_scene service..."); if (!client.waitForExistence(ros::Duration(1.0))) { ROS_INFO("Failed to get /soma_llsd/get_scene service!"); -// return; + return; } ROS_INFO("Got /soma_llsd/get_scene service"); @@ -2164,33 +2166,14 @@ void addSceneToLastMetaroom(std::string soma_id){ srv.request.scene_id = soma_id; if (!client.call(srv)) { ROS_ERROR("Failed to call service /soma_llsd/get_scene"); + return; } ROS_INFO("Got /soma_llsd/get_scene"); - cv_bridge::CvImagePtr rgb_ptr; - try{ rgb_ptr = cv_bridge::toCvCopy(srv.response.response.rgb_img, sensor_msgs::image_encodings::BGR8);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat rgb = rgb_ptr->image; - - cv_bridge::CvImagePtr depth_ptr; - try{ depth_ptr = cv_bridge::toCvCopy(srv.response.response.depth_img, sensor_msgs::image_encodings::MONO16);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat depth = depth_ptr->image; - - - //tf::poseEigenToMsg(Eigen::Affine3d(pose), scene_srv.request.robot_pose); - sensor_msgs::CameraInfo info = srv.response.response.camera_info; - - Eigen::Affine3d epose; - tf::poseMsgToEigen(srv.response.response.robot_pose, epose); - - reglib::Camera * cam = new reglib::Camera(); - if(info.K[0] > 0){ - cam->fx = info.K[0]; - cam->fy = info.K[4]; - cam->cx = info.K[2]; - cam->cy = info.K[5]; - } + reglib::RGBDFrame * frame = quasimodo_brain::getFrame(srv.response.response); + printf("frame->soma_id: %s\n",frame->soma_id.c_str()); + soma_llsd_msgs::Scene sc = quasimodo_brain::getScene(*np,frame,"",""); + printf("sc.id: %s\n",sc.id.c_str()); char buf [1024]; int counter = 0; @@ -2201,58 +2184,18 @@ void addSceneToLastMetaroom(std::string soma_id){ if (!file.exists()){break;} counter++; } - - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, 0, epose.matrix(),true,"",true); - frame->soma_id = soma_id; + printf("save to %s\n",buf); frame->save(std::string(buf)); - cam->save(std::string(buf)+"_camera"); + frame->camera->save(std::string(buf)+"_camera"); + delete frame->camera; delete frame; - delete cam; - -// reglib::Camera * cam2 = reglib::Camera::load(std::string(buf)+"_camera"); -// cam2->print(); - -// reglib::RGBDFrame * frame2 = reglib::RGBDFrame::load(cam2,std::string(buf)); -// frame2->show(true); - -// delete cam2; -// delete frame2; - - - //reglib::RGBDFrame * loadedframe = reglib::RGBDFrame::load(); - - //frame->show(true); - //model->frames.push_back(frame); - -// info.K.at(0) = K(0, 0); -// info.K.at(1) = K(0, 1); -// info.K.at(2) = K(0, 2); -// info.K.at(3) = K(1, 0); -// info.K.at(4) = K(1, 1); -// info.K.at(5) = K(1, 2); -// info.K.at(8) = K(2, 2); - -// info.P.at(0) = K(0, 0); -// info.P.at(1) = K(0, 1); -// info.P.at(2) = K(0, 2); -// info.P.at(4) = K(1, 0); -// info.P.at(5) = K(1, 1); -// info.P.at(6) = K(1, 2); -// info.P.at(10) = K(2, 2); -// info.height = rgb.rows; -// info.width = rgb.cols; -// scene_srv.request.camera_info = info; -// scene_srv.request.waypoint = waypoint; -// scene_srv.request.episode_id = episode_id; - -// cv::namedWindow( "rgb", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgb", rgb ); -// //cv::namedWindow( "normals", cv::WINDOW_AUTOSIZE ); cv::imshow( "normals", normals ); -// cv::namedWindow( "depth", cv::WINDOW_AUTOSIZE ); cv::imshow( "depth", depth ); -// //cv::namedWindow( "depthedges", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthedges", depthedges ); -// cv::waitKey(0); -// //if(stop){ cv::waitKey(0);}else{ cv::waitKey(30);} - -// exit(0); +} + +void add_soma_id_callback(const std_msgs::String::ConstPtr& msg){ + printf("================================================================================\n"); + printf("=============================add_soma_id_callback===============================\n"); + printf("================================================================================\n"); + addSceneToLastMetaroom(msg->data); } @@ -2272,6 +2215,7 @@ void setLargeStack(){ } int main(int argc, char** argv){ + bool baseSetting = true; bool once = false; @@ -2288,6 +2232,7 @@ int main(int argc, char** argv){ std::vector sendsubs; std::vector roomObservationSubs; std::vector processAndSendsubs; + std::vector soma_id_subs; std::vector trainMetarooms; @@ -2415,6 +2360,10 @@ int main(int argc, char** argv){ if(soma_segment_id_pubs.size() == 0){ soma_segment_id_pubs.push_back(n.advertise("/quasimodo/segmentation/out/soma_segment_id", 1000)); } + + if(soma_id_subs.size() == 0){ + soma_id_subs.push_back(n.subscribe("/quasimodo/segmentation/in/soma_segment_id", 1000,add_soma_id_callback)); + } } @@ -2422,10 +2371,8 @@ int main(int argc, char** argv){ printf("overall_folder: %s\n",overall_folder.c_str()); - - //addSceneToLastMetaroom("ed5f22eb-e6c0-426c-b905-4800780ca596"); - - +// addSceneToLastMetaroom("ed5f22eb-e6c0-426c-b905-4800780ca596"); +//exit(0); printf("testpaths: %i\n",testpaths.size()); for(unsigned int i = 0; i < testpaths.size(); i++){ testPath(testpaths[i]); diff --git a/quasimodo/quasimodo_brain/src/voPCD.cpp b/quasimodo/quasimodo_brain/src/voPCD.cpp index a7c49022..08f96e13 100644 --- a/quasimodo/quasimodo_brain/src/voPCD.cpp +++ b/quasimodo/quasimodo_brain/src/voPCD.cpp @@ -15,9 +15,9 @@ #include "quasimodo_msgs/retrieval_query_result.h" #include "quasimodo_msgs/retrieval_query.h" #include "quasimodo_msgs/retrieval_result.h" -#include "soma2_msgs/SOMA2Object.h" +//#include "soma2_msgs/SOMA2Object.h" -#include "soma_manager/SOMA2InsertObjs.h" +//#include "soma_manager/SOMA2InsertObjs.h" //#include "modelupdater/ModelUpdater.h" //#include "/home/johane/catkin_ws_dyn/src/quasimodo_models/include/modelupdater/ModelUpdater.h" diff --git a/quasimodo/quasimodo_models/include/core/quasimodo_point.h b/quasimodo/quasimodo_models/include/core/quasimodo_point.h index 412a4b82..0fb49fef 100644 --- a/quasimodo/quasimodo_models/include/core/quasimodo_point.h +++ b/quasimodo/quasimodo_models/include/core/quasimodo_point.h @@ -8,29 +8,35 @@ struct QuasimodoPointType { - PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4]) - union - { - struct + //PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) + //PCL_ADD_RGB;//Add color to the point + PCL_ADD_NORMAL4D; // This adds the member normal[3] which can also be accessed using the point (which is float[4]) + union { - float ce_x; - float ce_y; - float de_x; - float de_y; + struct + { + float ce_x; + float ce_y; + float de_x; + float de_y; + }; + float data_t[4]; }; - float data_t[4]; - }; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned + EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment POINT_CLOUD_REGISTER_POINT_STRUCT (QuasimodoPointType, +// (float, x, x) +// (float, y, y) +// (float, z, z) (float, normal_x, normal_x) (float, normal_y, normal_y) (float, normal_z, normal_z) + //(uint32_t, rgba, rgba) (float, ce_x, ce_x) (float, ce_y, ce_y) (float, de_x, de_x) (float, de_y, de_y) -) + ) #endif diff --git a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp index 2982af08..89f8318c 100644 --- a/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp +++ b/quasimodo/quasimodo_models/src/core/RGBDFrame.cpp @@ -921,24 +921,25 @@ pcl::PointCloud::Ptr RGBDFrame::getSmallPCLcloud(){ pcl::PointCloud::Ptr RGBDFrame::getPCLcloud(){ unsigned char * rgbdata = (unsigned char *)rgb.data; unsigned short * depthdata = (unsigned short *)depth.data; + float * normalsdata = (float *)normals.data; const unsigned int width = camera->width; const unsigned int height = camera->height; const double idepth = camera->idepth_scale; const double cx = camera->cx; const double cy = camera->cy; const double ifx = 1.0/camera->fx; const double ify = 1.0/camera->fy; - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr normals (new pcl::PointCloud); + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); cloud->width = width; cloud->height = height; cloud->points.resize(width*height); for(unsigned int w = 0; w < width; w++){ for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; + unsigned int ind = h*width+w; double z = idepth*double(depthdata[ind]); - pcl::PointXYZRGB p; + pcl::PointXYZRGBNormal p; p.b = rgbdata[3*ind+0]; p.g = rgbdata[3*ind+1]; p.r = rgbdata[3*ind+2]; @@ -951,49 +952,82 @@ pcl::PointCloud::Ptr RGBDFrame::getPCLcloud(){ p.y = NAN; p.z = NAN; } + p.normal_x = normalsdata[3*ind+0]; + p.normal_y = normalsdata[3*ind+1]; + p.normal_z = normalsdata[3*ind+2]; cloud->points[ind] = p; } } + return cloud; - pcl::IntegralImageNormalEstimation ne; - ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); - ne.setMaxDepthChangeFactor(0.02f); - ne.setNormalSmoothingSize(10.0f); - ne.setInputCloud(cloud); - ne.compute(*normals); - - pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); - cloud_ptr->width = width; - cloud_ptr->height = height; - cloud_ptr->points.resize(width*height); - - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - pcl::PointXYZRGBNormal & p0 = cloud_ptr->points[ind]; - pcl::PointXYZRGB p1 = cloud->points[ind]; - pcl::Normal p2 = normals->points[ind]; - - p0.x = p1.x; - p0.y = p1.y; - p0.z = p1.z; - - if(depthdata[ind] == 0){ - p0.x = 0; - p0.y = 0; - p0.z = 0; - } - p0.r = p1.r; - p0.g = p1.g; - p0.b = p1.b; - p0.normal_x = p2.normal_x; - p0.normal_y = p2.normal_y; - p0.normal_z = p2.normal_z; - } - } - return cloud_ptr; +// pcl::PointCloud::Ptr cloud (new pcl::PointCloud); +// pcl::PointCloud::Ptr normals (new pcl::PointCloud); +// cloud->width = width; +// cloud->height = height; +// cloud->points.resize(width*height); + +// for(unsigned int w = 0; w < width; w++){ +// for(unsigned int h = 0; h < height;h++){ +// int ind = h*width+w; +// double z = idepth*double(depthdata[ind]); + +// pcl::PointXYZRGB p; +// p.b = rgbdata[3*ind+0]; +// p.g = rgbdata[3*ind+1]; +// p.r = rgbdata[3*ind+2]; +// if(z > 0){ +// p.x = (double(w) - cx) * z * ifx; +// p.y = (double(h) - cy) * z * ify; +// p.z = z; +// }else{ +// p.x = NAN; +// p.y = NAN; +// p.z = NAN; +// } +// cloud->points[ind] = p; +// } +// } + +// pcl::IntegralImageNormalEstimation ne; +// ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT); +// ne.setMaxDepthChangeFactor(0.02f); +// ne.setNormalSmoothingSize(10.0f); +// ne.setInputCloud(cloud); +// ne.compute(*normals); + +// pcl::PointCloud::Ptr cloud_ptr (new pcl::PointCloud); +// cloud_ptr->width = width; +// cloud_ptr->height = height; +// cloud_ptr->points.resize(width*height); + +// for(unsigned int w = 0; w < width; w++){ +// for(unsigned int h = 0; h < height;h++){ +// int ind = h*width+w; +// pcl::PointXYZRGBNormal & p0 = cloud_ptr->points[ind]; +// pcl::PointXYZRGB p1 = cloud->points[ind]; +// pcl::Normal p2 = normals->points[ind]; + +// p0.x = p1.x; +// p0.y = p1.y; +// p0.z = p1.z; + +// if(depthdata[ind] == 0){ +// p0.x = 0; +// p0.y = 0; +// p0.z = 0; +// } + +// p0.r = p1.r; +// p0.g = p1.g; +// p0.b = p1.b; +// p0.normal_x = p2.normal_x; +// p0.normal_y = p2.normal_y; +// p0.normal_z = p2.normal_z; +// } +// } +// return cloud_ptr; } void RGBDFrame::savePCD(std::string path, Eigen::Matrix4d pose){ From 3fa705e393cb5f30e67dc2c4c10cbd310a6c7acf Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Thu, 24 Nov 2016 16:50:28 +0100 Subject: [PATCH 235/255] ... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 4 ++++ quasimodo/quasimodo_brain/src/Util/Util.h | 2 ++ .../src/metaroom_additional_view_processing.cpp | 4 ++-- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index ec3f46e1..298f0fd2 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -2,6 +2,10 @@ namespace quasimodo_brain { +std::string initSegment(ros::NodeHandle& n, reglib::Model * model){ + return ""; +} + soma_llsd_msgs::Scene getScene(ros::NodeHandle & n, reglib::RGBDFrame * frame, std::string current_waypointid, std::string roomRunNumber){ soma_llsd_msgs::Scene sc; if(frame->soma_id.length() > 0){//Frame already connected to scene, get from db diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index f9ba4a57..029cc599 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -42,6 +42,8 @@ namespace quasimodo_brain { +std::string initSegment(ros::NodeHandle& n, reglib::Model * model); + soma_llsd_msgs::Scene getScene(ros::NodeHandle& n, reglib::RGBDFrame * frame, std::string current_waypointid = "", std::string roomRunNumber = ""); reglib::Camera * getCam(sensor_msgs::CameraInfo & info); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 4f3c89e5..115c6450 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -2172,8 +2172,8 @@ void addSceneToLastMetaroom(std::string soma_id){ reglib::RGBDFrame * frame = quasimodo_brain::getFrame(srv.response.response); printf("frame->soma_id: %s\n",frame->soma_id.c_str()); - soma_llsd_msgs::Scene sc = quasimodo_brain::getScene(*np,frame,"",""); - printf("sc.id: %s\n",sc.id.c_str()); +// soma_llsd_msgs::Scene sc = quasimodo_brain::getScene(*np,frame,"",""); +// printf("sc.id: %s\n",sc.id.c_str()); char buf [1024]; int counter = 0; From 44e87f74290d82cb8de8feefc6dcdde2f684170a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 25 Nov 2016 09:59:28 +0100 Subject: [PATCH 236/255] bugfix and working on soma stuff --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 57 +++++++++++++++++++ quasimodo/quasimodo_brain/src/Util/Util.h | 2 + .../metaroom_additional_view_processing.cpp | 6 +- 3 files changed, 63 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 298f0fd2..9610da87 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -2,7 +2,64 @@ namespace quasimodo_brain { +double score; +unsigned long id; + +//int last_changed; +//std::string savePath; +//std::vector points; +//std::vector< std::vector > all_keypoints; +//std::vector< cv::Mat > all_descriptors; +//std::vector relativeposes; +//std::vector frames; +//std::vector modelmasks; +//std::vector rep_relativeposes; +//std::vector rep_frames; +//std::vector rep_modelmasks; +//double total_scores; +//std::vector > scores; +//std::vector submodels; +//std::vector submodels_relativeposes; +//std::vector > submodels_scores; + +std::string getPoseString(Eigen::Matrix4d pose){ + char buf [1024]; + for(unsigned int i = 0; i < 4; i ++){ + for(unsigned int j = 0; j < 4; j ++){ + sprintf(buf,"%6.6f ",pose(i,j)); + } + } + return std::string(buf); +} + std::string initSegment(ros::NodeHandle& n, reglib::Model * model){ + std::vector< std::string > scid; + std::vector< cv::Mat > masks; + std::vector< Eigen::Matrix4d, Eigen::aligned_allocator > > poses; + for(unsigned int i = 0; i < model->frames.size(); i++){ + soma_llsd_msgs::Scene sc = getScene(n,model->frames[i]); + poses.push_back(model->relativeposes[i]); + masks.push_back(model->modelmasks[i]->getMask()); + scid.push_back(sc.id); + } + + soma_llsd_msgs::Segment segment; + if(quasimodo_conversions::add_masks_to_soma_segment(n,scid, masks,poses,segment)){ + std::string id = segment.id; + std::string metadata = ""; + for(unsigned int i = 0; i < model->submodels.size(); i++){ + metadata += ""; + std::string subid = initSegment(n, model->submodels[i]); + metadata += subid; + metadata += ""; + metadata += getPoseString(model->submodels_relativeposes[i]); + metadata += "<\relativepose>"; + metadata += "<\submodel>"; + } + printf("metadata: %s\n",metadata.c_str()); + return id; + } + return ""; } diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index 029cc599..498cdd5e 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -39,6 +39,8 @@ #include #include +#include +#include namespace quasimodo_brain { diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 115c6450..17c58b3e 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -716,7 +716,7 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: } printf("fullmodel->points.size() = %i\n",fullmodel->points.size()); - + quasimodo_brain::writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); // delete reg; // delete mu; // delete sweep; @@ -744,10 +744,12 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); fullmodel = processAV(path,compute_edges,saveVisuals_sp); //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); - quasimodo_brain::writeXml(sweep_folder+"ViewGroup.xml",fullmodel->frames,fullmodel->relativeposes); + } //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); */ + //initSegment(*np,fullmodel); +exit(0); return fullmodel; } From 10290a4440d2d6dde1c772cf5283854e62bd0110 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 25 Nov 2016 10:10:23 +0100 Subject: [PATCH 237/255] ... --- .../quasimodo_brain/src/metaroom_additional_view_processing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 17c58b3e..7aebe351 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -749,7 +749,7 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); */ //initSegment(*np,fullmodel); -exit(0); +//exit(0); return fullmodel; } From dd6892f4184e677e66fa7296a084933759786a45 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 25 Nov 2016 11:22:05 +0100 Subject: [PATCH 238/255] ... --- quasimodo/quasimodo_brain/CMakeLists.txt | 10 ++--- quasimodo/quasimodo_brain/src/Util/Util.cpp | 42 ++++++++++++------- .../metaroom_additional_view_processing.cpp | 5 ++- .../quasimodo_models/include/model/Model.h | 2 + .../quasimodo_models/src/model/Model.cpp | 4 ++ 5 files changed, 42 insertions(+), 21 deletions(-) diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 0125fb78..18d9eb74 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 2.8.3) project(quasimodo_brain) -find_package(catkin REQUIRED COMPONENTS roscpp soma_msgs object_manager object_manager_msgs semantic_map_msgs soma_manager local_metric_map_people_rejection quasimodo_msgs quasimodo_conversions quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge) +find_package(catkin REQUIRED COMPONENTS quasimodo_models roscpp soma_msgs object_manager object_manager_msgs semantic_map_msgs soma_manager local_metric_map_people_rejection quasimodo_msgs quasimodo_conversions quasimodo_models message_runtime pcl_ros metaroom_xml_parser eigen_conversions cv_bridge) set(CMAKE_CXX_FLAGS "-O4 -g -pg -Wunknown-pragmas -Wno-unknown-pragmas -Wsign-compare -fPIC -std=c++0x -o popcnt -mssse3") @@ -126,19 +126,19 @@ target_link_libraries( test_segment quasimodo_brain_util quasimodo_ModelDatabase add_executable( segment_room_pairs src/segment_room_pairs.cpp) add_dependencies( segment_room_pairs roscpp quasimodo_msgs_generate_messages_cpp) -target_link_libraries( segment_room_pairs quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries( segment_room_pairs quasimodo_ModelUpdater quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) add_executable( test_segment_room_pairs src/test_segment_room_pairs.cpp) add_dependencies( test_segment_room_pairs roscpp quasimodo_msgs_generate_messages_cpp) -target_link_libraries( test_segment_room_pairs quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries( test_segment_room_pairs quasimodo_ModelUpdater quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) add_executable( segmentationserver src/segmentationserver.cpp) add_dependencies( segmentationserver roscpp quasimodo_msgs_generate_messages_cpp) -target_link_libraries( segmentationserver quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries( segmentationserver quasimodo_brain_util quasimodo_ModelUpdater quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) add_executable( metaroom_additional_view_processing src/metaroom_additional_view_processing.cpp) add_dependencies( metaroom_additional_view_processing roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp) -target_link_libraries( metaroom_additional_view_processing quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries( metaroom_additional_view_processing quasimodo_ModelUpdater quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) add_executable( AnnotationTool src/AnnotationTool.cpp) add_dependencies( AnnotationTool roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 9610da87..ac99456a 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -2,21 +2,13 @@ namespace quasimodo_brain { -double score; -unsigned long id; - -//int last_changed; //std::string savePath; //std::vector points; -//std::vector< std::vector > all_keypoints; -//std::vector< cv::Mat > all_descriptors; -//std::vector relativeposes; -//std::vector frames; -//std::vector modelmasks; -//std::vector rep_relativeposes; + +//std::vector rep_relativeposes; //std::vector rep_frames; //std::vector rep_modelmasks; -//double total_scores; + //std::vector > scores; //std::vector submodels; //std::vector submodels_relativeposes; @@ -37,6 +29,7 @@ std::string initSegment(ros::NodeHandle& n, reglib::Model * model){ std::vector< cv::Mat > masks; std::vector< Eigen::Matrix4d, Eigen::aligned_allocator > > poses; for(unsigned int i = 0; i < model->frames.size(); i++){ + printf("frame: %i\n",i); soma_llsd_msgs::Scene sc = getScene(n,model->frames[i]); poses.push_back(model->relativeposes[i]); masks.push_back(model->modelmasks[i]->getMask()); @@ -46,15 +39,36 @@ std::string initSegment(ros::NodeHandle& n, reglib::Model * model){ soma_llsd_msgs::Segment segment; if(quasimodo_conversions::add_masks_to_soma_segment(n,scid, masks,poses,segment)){ std::string id = segment.id; - std::string metadata = ""; + model->soma_id = id; + std::string metadata = ""; + metadata += ""; + metadata += std::to_string(model->last_changed); + metadata += "<\\last_changed>"; + + metadata += ""; + metadata += std::to_string(model->id); + metadata += "<\\id>"; + + metadata += ""; + metadata += std::to_string(model->score); + metadata += "<\\score>"; + + metadata += ""; + metadata += std::to_string(model->total_scores); + metadata += "<\\total_scores>"; + + metadata += ""; + metadata += model->pointspath; + metadata += "<\\pointspath>"; + for(unsigned int i = 0; i < model->submodels.size(); i++){ metadata += ""; std::string subid = initSegment(n, model->submodels[i]); metadata += subid; metadata += ""; metadata += getPoseString(model->submodels_relativeposes[i]); - metadata += "<\relativepose>"; - metadata += "<\submodel>"; + metadata += "<\\relativepose>"; + metadata += "<\\submodel>"; } printf("metadata: %s\n",metadata.c_str()); return id; diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 7aebe351..c6d20916 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -748,8 +748,9 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: } //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); */ - //initSegment(*np,fullmodel); -//exit(0); + std::string soma_id = quasimodo_brain::initSegment(*np,fullmodel); + printf("soma_id: %i\n",soma_id.c_str()); +exit(0); return fullmodel; } diff --git a/quasimodo/quasimodo_models/include/model/Model.h b/quasimodo/quasimodo_models/include/model/Model.h index bab18aaa..3a7543c0 100644 --- a/quasimodo/quasimodo_models/include/model/Model.h +++ b/quasimodo/quasimodo_models/include/model/Model.h @@ -33,7 +33,9 @@ using namespace Eigen; int last_changed; std::string savePath; + std::string soma_id; + std::string pointspath; std::vector points; std::vector< std::vector > all_keypoints; diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 11c65b73..55772294 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -14,6 +14,8 @@ Model::Model(){ id = model_id_counter++; last_changed = -1; savePath = ""; + soma_id = ""; + pointspath = ""; } Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){ @@ -32,6 +34,8 @@ Model::Model(RGBDFrame * frame, cv::Mat mask, Eigen::Matrix4d pose){ modelmasks.push_back(new ModelMask(mask)); savePath = ""; + soma_id = ""; + pointspath = ""; //recomputeModelPoints(); } From 6edfdfb430fee1cf622169cc15ab496092e283e6 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 25 Nov 2016 11:23:51 +0100 Subject: [PATCH 239/255] ... --- .../src/metaroom_additional_view_processing.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index c6d20916..03b43c1f 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -748,9 +748,9 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: } //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); */ - std::string soma_id = quasimodo_brain::initSegment(*np,fullmodel); - printf("soma_id: %i\n",soma_id.c_str()); -exit(0); +// std::string soma_id = quasimodo_brain::initSegment(*np,fullmodel); +// printf("soma_id: %i\n",soma_id.c_str()); +//exit(0); return fullmodel; } From c2f10c614e2bc9a863790b2df56be65b226f906d Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 25 Nov 2016 12:20:28 +0100 Subject: [PATCH 240/255] ... --- quasimodo/quasimodo_brain/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 18d9eb74..9451bf85 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -96,7 +96,7 @@ target_link_libraries( robot_listener image_geometry cpp_common roscpp rosconso add_executable( modelserver src/modelserver.cpp) add_dependencies( modelserver roscpp quasimodo_msgs_generate_messages_cpp) -target_link_libraries( modelserver quasimodo_brain_util quasimodo_ModelDatabase image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries( modelserver quasimodo_superpoint quasimodo_brain_util quasimodo_ModelDatabase image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) add_executable( massregPCD src/massregPCD.cpp) @@ -138,7 +138,7 @@ target_link_libraries( segmentationserver quasimodo_brain_util quasimodo_ModelUp add_executable( metaroom_additional_view_processing src/metaroom_additional_view_processing.cpp) add_dependencies( metaroom_additional_view_processing roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp) -target_link_libraries( metaroom_additional_view_processing quasimodo_ModelUpdater quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) +target_link_libraries( metaroom_additional_view_processing quasimodo_superpoint quasimodo_ModelUpdater quasimodo_brain_util quasimodo_ModelDatabase quasimodo_Mesher image_geometry cpp_common roscpp rosconsole tf_conversions metaroom_xml_parser ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} ${catkin_LIBRARIES}) add_executable( AnnotationTool src/AnnotationTool.cpp) add_dependencies( AnnotationTool roscpp quasimodo_msgs_generate_messages_cpp object_manager_msgs_generate_messages_cpp semantic_map_msgs_generate_messages_cpp) From 0eccd5a6d330f891be95c99c4e0caaf230873994 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 25 Nov 2016 13:23:08 +0100 Subject: [PATCH 241/255] ... --- quasimodo/quasimodo_retrieval/CMakeLists.txt | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/quasimodo/quasimodo_retrieval/CMakeLists.txt b/quasimodo/quasimodo_retrieval/CMakeLists.txt index 484733bf..ace78e87 100644 --- a/quasimodo/quasimodo_retrieval/CMakeLists.txt +++ b/quasimodo/quasimodo_retrieval/CMakeLists.txt @@ -180,13 +180,13 @@ add_executable(quasimodo_retrieval_node src/quasimodo_retrieval_node.cpp) add_executable(quasimodo_retrieve_observation src/quasimodo_retrieve_observation.cpp) add_executable(quasimodo_logging_server src/quasimodo_logging_server.cpp) -add_dependencies(quasimodo_retrieval_server quasimodo_msgs_generate_messages_cpp) -add_dependencies(quasimodo_retrieval_publisher quasimodo_msgs_generate_messages_cpp) -add_dependencies(quasimodo_visualization_server quasimodo_msgs_generate_messages_cpp) -add_dependencies(quasimodo_visualize_model quasimodo_msgs_generate_messages_cpp) -add_dependencies(quasimodo_retrieval_node quasimodo_msgs_generate_messages_cpp) -add_dependencies(quasimodo_retrieve_observation quasimodo_msgs_generate_messages_cpp) -add_dependencies(quasimodo_logging_server quasimodo_msgs_generate_messages_cpp soma_msgs_generate_messages_cpp) +add_dependencies(quasimodo_retrieval_server mongodb_store quasimodo_msgs_generate_messages_cpp) +add_dependencies(quasimodo_retrieval_publisher mongodb_store quasimodo_msgs_generate_messages_cpp) +add_dependencies(quasimodo_visualization_server mongodb_store quasimodo_msgs_generate_messages_cpp) +add_dependencies(quasimodo_visualize_model mongodb_store quasimodo_msgs_generate_messages_cpp) +add_dependencies(quasimodo_retrieval_node mongodb_store quasimodo_msgs_generate_messages_cpp) +add_dependencies(quasimodo_retrieve_observation mongodb_store quasimodo_msgs_generate_messages_cpp) +add_dependencies(quasimodo_logging_server mongodb_store quasimodo_msgs_generate_messages_cpp soma_msgs_generate_messages_cpp) ## Add cmake target dependencies of the executable ## same as for the library above From 9eb047f76d20c24309836fc911e094d2c067f46e Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 25 Nov 2016 15:58:20 +0100 Subject: [PATCH 242/255] ... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 219 +++++++++++++----- quasimodo/quasimodo_brain/src/Util/Util.h | 6 + .../metaroom_additional_view_processing.cpp | 12 + 3 files changed, 176 insertions(+), 61 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index ac99456a..02fa525b 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -2,9 +2,7 @@ namespace quasimodo_brain { -//std::string savePath; -//std::vector points; - +//Save? //std::vector rep_relativeposes; //std::vector rep_frames; //std::vector rep_modelmasks; @@ -18,27 +16,51 @@ std::string getPoseString(Eigen::Matrix4d pose){ char buf [1024]; for(unsigned int i = 0; i < 4; i ++){ for(unsigned int j = 0; j < 4; j ++){ - sprintf(buf,"%6.6f ",pose(i,j)); + if(i == 0 && j == 0){ + sprintf(buf,"%10.10f",pose(i,j)); + }else{ + std::string str = std::string(buf); + sprintf(buf,"%s %10.10f",str.c_str(),pose(i,j)); + } } } return std::string(buf); } + +Eigen::Matrix4d getPoseFromString(std::string str){ + Eigen::Matrix4d pose; + for(unsigned int i = 0; i < 4; i ++){ + for(unsigned int j = 0; j < 4; j ++){ + std::size_t found = str.find(" "); + if (found!=std::string::npos){ + pose(i,j) = atof(str.substr(0,found).c_str()); + str = str.substr(found+1,str.length()-found); + }else{ + pose(i,j) = atof(str.c_str()); + } + } + } + return pose; +} + std::string initSegment(ros::NodeHandle& n, reglib::Model * model){ - std::vector< std::string > scid; - std::vector< cv::Mat > masks; - std::vector< Eigen::Matrix4d, Eigen::aligned_allocator > > poses; - for(unsigned int i = 0; i < model->frames.size(); i++){ + std::vector< std::string > scid; + std::vector< cv::Mat > masks; + std::vector< Eigen::Matrix4d, Eigen::aligned_allocator > > poses; + for(unsigned int i = 0; i < model->frames.size(); i++){ printf("frame: %i\n",i); - soma_llsd_msgs::Scene sc = getScene(n,model->frames[i]); - poses.push_back(model->relativeposes[i]); - masks.push_back(model->modelmasks[i]->getMask()); - scid.push_back(sc.id); - } - - soma_llsd_msgs::Segment segment; - if(quasimodo_conversions::add_masks_to_soma_segment(n,scid, masks,poses,segment)){ - std::string id = segment.id; + soma_llsd_msgs::Scene sc = getScene(n,model->frames[i]); + model->frames[i]->soma_id = sc.id; + poses.push_back(model->relativeposes[i]); + masks.push_back(model->modelmasks[i]->getMask()); + scid.push_back(sc.id); + // getPoseFromString(getPoseString(model->relativeposes[i])); + } + + soma_llsd_msgs::Segment segment; + if(quasimodo_conversions::add_masks_to_soma_segment(n,scid, masks,poses,segment)){ + std::string id = segment.id; model->soma_id = id; std::string metadata = ""; metadata += ""; @@ -61,22 +83,97 @@ std::string initSegment(ros::NodeHandle& n, reglib::Model * model){ metadata += model->pointspath; metadata += "<\\pointspath>"; - for(unsigned int i = 0; i < model->submodels.size(); i++){ - metadata += ""; - std::string subid = initSegment(n, model->submodels[i]); - metadata += subid; - metadata += ""; + for(unsigned int i = 0; i < model->submodels.size(); i++){ + metadata += ""; + std::string subid = initSegment(n, model->submodels[i]); + metadata += subid; + metadata += ""; metadata += getPoseString(model->submodels_relativeposes[i]); metadata += "<\\relativepose>"; metadata += "<\\submodel>"; - } - printf("metadata: %s\n",metadata.c_str()); - return id; - } + } + printf("metadata: %s\n",metadata.c_str()); + return id; + } return ""; } +reglib::Model * getModelFromSegment(ros::NodeHandle& n, std::string segment_id){ + reglib::Model * model = new reglib::Model(); + + ros::ServiceClient client = n.serviceClient("/soma_llsd/get_segment"); + ROS_INFO("Waiting for /soma_llsd/get_segment service..."); + if (!client.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/get_segment service!"); + return model; + } + ROS_INFO("Got /soma_llsd/get_segment service"); + + soma_llsd::GetSegment srv; + srv.request.segment_id = model->soma_id; + if (client.call(srv)) { + ROS_INFO("Got /soma_llsd/get_segment"); + return model; + } + + //Read frames etc + //Read parameters + //Read submodels + return model; + // std::vector< std::string > scid; + // std::vector< cv::Mat > masks; + // std::vector< Eigen::Matrix4d, Eigen::aligned_allocator > > poses; + // for(unsigned int i = 0; i < model->frames.size(); i++){ + // printf("frame: %i\n",i); + // soma_llsd_msgs::Scene sc = getScene(n,model->frames[i]); + // poses.push_back(model->relativeposes[i]); + // masks.push_back(model->modelmasks[i]->getMask()); + // scid.push_back(sc.id); + //// getPoseFromString(getPoseString(model->relativeposes[i])); + // } + + // soma_llsd_msgs::Segment segment; + // if(quasimodo_conversions::add_masks_to_soma_segment(n,scid, masks,poses,segment)){ + // std::string id = segment.id; + // model->soma_id = id; + // std::string metadata = ""; + // metadata += ""; + // metadata += std::to_string(model->last_changed); + // metadata += "<\\last_changed>"; + + // metadata += ""; + // metadata += std::to_string(model->id); + // metadata += "<\\id>"; + + // metadata += ""; + // metadata += std::to_string(model->score); + // metadata += "<\\score>"; + + // metadata += ""; + // metadata += std::to_string(model->total_scores); + // metadata += "<\\total_scores>"; + + // metadata += ""; + // metadata += model->pointspath; + // metadata += "<\\pointspath>"; + + // for(unsigned int i = 0; i < model->submodels.size(); i++){ + // metadata += ""; + // std::string subid = initSegment(n, model->submodels[i]); + // metadata += subid; + // metadata += ""; + // metadata += getPoseString(model->submodels_relativeposes[i]); + // metadata += "<\\relativepose>"; + // metadata += "<\\submodel>"; + // } + // printf("metadata: %s\n",metadata.c_str()); + // return id; + // } + + // return ""; +} + soma_llsd_msgs::Scene getScene(ros::NodeHandle & n, reglib::RGBDFrame * frame, std::string current_waypointid, std::string roomRunNumber){ soma_llsd_msgs::Scene sc; if(frame->soma_id.length() > 0){//Frame already connected to scene, get from db @@ -846,7 +943,7 @@ Eigen::Matrix4d getMat(tf::StampedTransform tf){ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath){ int slash_pos = sweep_xml.find_last_of("/"); std::string sweep_folder = sweep_xml.substr(0, slash_pos) + "/"; -// printf("load_metaroom_model(%s)\n",sweep_folder.c_str()); + // printf("load_metaroom_model(%s)\n",sweep_folder.c_str()); SimpleXMLParser parser; std::vector dummy; @@ -866,9 +963,9 @@ reglib::Model * load_metaroom_model(std::string sweep_xml, std::string savePath) std::vector current_room_frames; -// printf("roomData.vIntermediateRoomClouds.size() = %i\n",roomData.vIntermediateRoomClouds.size()); -// printf("roomData.vIntermediateRoomCloudCamParamsCorrected.size() = %i\n",roomData.vIntermediateRoomCloudCamParamsCorrected.size()); -// printf("roomData.vIntermediateRoomCloudTransformsRegistered.size() = %i\n",roomData.vIntermediateRoomCloudTransformsRegistered.size()); + // printf("roomData.vIntermediateRoomClouds.size() = %i\n",roomData.vIntermediateRoomClouds.size()); + // printf("roomData.vIntermediateRoomCloudCamParamsCorrected.size() = %i\n",roomData.vIntermediateRoomCloudCamParamsCorrected.size()); + // printf("roomData.vIntermediateRoomCloudTransformsRegistered.size() = %i\n",roomData.vIntermediateRoomCloudTransformsRegistered.size()); for (size_t i=0; i loadModelsPCDs(std::string path){ reglib::Model * model = new reglib::Model(); -// for(unsigned int i = 0; i < msg.local_poses.size(); i++){ -// sensor_msgs::CameraInfo camera = msg.frames[i].camera; -// ros::Time capture_time = msg.frames[i].capture_time; -// geometry_msgs::Pose pose = msg.frames[i].pose; + // for(unsigned int i = 0; i < msg.local_poses.size(); i++){ + // sensor_msgs::CameraInfo camera = msg.frames[i].camera; + // ros::Time capture_time = msg.frames[i].capture_time; + // geometry_msgs::Pose pose = msg.frames[i].pose; -// cv_bridge::CvImagePtr rgb_ptr; -// try{ rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);} -// catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} -// cv::Mat rgb = rgb_ptr->image; + // cv_bridge::CvImagePtr rgb_ptr; + // try{ rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);} + // catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + // cv::Mat rgb = rgb_ptr->image; -// cv_bridge::CvImagePtr depth_ptr; -// try{ depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);} -// catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} -// cv::Mat depth = depth_ptr->image; + // cv_bridge::CvImagePtr depth_ptr; + // try{ depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);} + // catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} + // cv::Mat depth = depth_ptr->image; -// Eigen::Affine3d epose; -// tf::poseMsgToEigen(pose, epose); + // Eigen::Affine3d epose; + // tf::poseMsgToEigen(pose, epose); -// reglib::Camera * cam = new reglib::Camera(); -// if(camera.K[0] > 0){ -// cam->fx = camera.K[0]; -// cam->fy = camera.K[4]; -// cam->cx = camera.K[2]; -// cam->cy = camera.K[5]; -// } + // reglib::Camera * cam = new reglib::Camera(); + // if(camera.K[0] > 0){ + // cam->fx = camera.K[0]; + // cam->fy = camera.K[4]; + // cam->cx = camera.K[2]; + // cam->cy = camera.K[5]; + // } -// } -// + // } + // for(unsigned int j = 0; j < cloudspaths.size(); j++){ std::string cloudpath = cloudspaths[j]; @@ -1321,14 +1418,14 @@ std::vector loadModelsPCDs(std::string path){ reglib::Camera * cam = new reglib::Camera();//figure out cam params //TODO figure out optical centre and focal lengths from cloud... -// for(unsigned int w = 0; w < width; w++){ -// for(unsigned int w = 0; w < width; w++){ -// pcl::PointXYZRGB p = cloud->points[k]; -// float x = p.x; -// float y = p.y; -// float z = p.z; -// } -// } + // for(unsigned int w = 0; w < width; w++){ + // for(unsigned int w = 0; w < width; w++){ + // pcl::PointXYZRGB p = cloud->points[k]; + // float x = p.x; + // float y = p.y; + // float z = p.z; + // } + // } for(unsigned int k = 0; k < width*height; k++){ pcl::PointXYZRGB p = cloud->points[k]; diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index 498cdd5e..8b6ef659 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -38,13 +38,19 @@ #include #include +#include #include #include #include namespace quasimodo_brain { +std::string getPoseString(Eigen::Matrix4d pose); + +Eigen::Matrix4d getPoseFromString(std::string str); + std::string initSegment(ros::NodeHandle& n, reglib::Model * model); +reglib::Model * getModelFromSegment(ros::NodeHandle& n, std::string segment_id); soma_llsd_msgs::Scene getScene(ros::NodeHandle& n, reglib::RGBDFrame * frame, std::string current_waypointid = "", std::string roomRunNumber = ""); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 03b43c1f..316ac005 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -748,8 +748,19 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: } //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); */ + double startTime_store = reglib::getTime(); +// fullmodel->pointspath = sweep_folder+"/fullmodel_superpoints.bin"; // std::string soma_id = quasimodo_brain::initSegment(*np,fullmodel); // printf("soma_id: %i\n",soma_id.c_str()); +// printf("add time: %6.6f\n",reglib::getTime()-startTime_store); + +// startTime_store = reglib::getTime(); +// std::string soma_id2 = quasimodo_brain::initSegment(*np,fullmodel); +// printf("soma_id2: %i\n",soma_id2.c_str()); +// printf("add2 time: %6.6f\n",reglib::getTime()-startTime_store); + +// reglib::Model * test = quasimodo_brain::getModelFromSegment(*np,soma_id); +// printf("load time: %6.6f\n",reglib::getTime()-startTime_store); //exit(0); return fullmodel; } @@ -2366,6 +2377,7 @@ int main(int argc, char** argv){ if(soma_id_subs.size() == 0){ soma_id_subs.push_back(n.subscribe("/quasimodo/segmentation/in/soma_segment_id", 1000,add_soma_id_callback)); + soma_id_subs.push_back(n.subscribe("/surface_based_object_learning/scenes", 1000,add_soma_id_callback)); } } From 0b21a28af6af2340e3d9f213db46ed7e52155861 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 25 Nov 2016 16:41:40 +0100 Subject: [PATCH 243/255] ... --- .../src/metaroom_additional_view_processing.cpp | 16 ++++++++++++---- .../src/quasimodo_conversions/conversions.cpp | 2 +- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 316ac005..7dd41d9d 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -748,7 +748,7 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: } //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); */ - double startTime_store = reglib::getTime(); +// double startTime_store = reglib::getTime(); // fullmodel->pointspath = sweep_folder+"/fullmodel_superpoints.bin"; // std::string soma_id = quasimodo_brain::initSegment(*np,fullmodel); // printf("soma_id: %i\n",soma_id.c_str()); @@ -1694,12 +1694,10 @@ void sendRoomToSomaLLSD(std::string path){ cloud->width = width; cloud->height = height; cloud->points.resize(width*height); - for(unsigned int w = 0; w < width; w++){ for(unsigned int h = 0; h < height;h++){ int ind = h*width+w; double z = idepth*double(depthdata[ind]); - pcl::PointXYZRGB p; p.b = rgbdata[3*ind+0]; p.g = rgbdata[3*ind+1]; @@ -2209,7 +2207,17 @@ void add_soma_id_callback(const std_msgs::String::ConstPtr& msg){ printf("================================================================================\n"); printf("=============================add_soma_id_callback===============================\n"); printf("================================================================================\n"); - addSceneToLastMetaroom(msg->data); + if(msg->data.compare("done") == 0){ + vector sweep_xmls = semantic_map_load_utilties::getSweepXmls(overall_folder); + if(sweep_xmls.size() == 0){return ;} + std::string lastSweep = sweep_xmls.back(); + //if(prevind >= 0){ + sendRoomToSomaLLSD(lastSweep); + sendMetaroomToServer(lastSweep); + //} + }else{ + addSceneToLastMetaroom(msg->data); + } } diff --git a/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp index 821ea3e7..7b54b69a 100644 --- a/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp +++ b/quasimodo/quasimodo_conversions/src/quasimodo_conversions/conversions.cpp @@ -200,7 +200,7 @@ bool raw_frames_to_soma_scene(ros::NodeHandle& n, const cv::Mat& rgb, const cv:: convert_to_img_msg(rgb, scene_srv.request.rgb_img); convert_to_depth_msg(depth, scene_srv.request.depth_img); - pcl::toROSMsg(*cloud, scene_srv.request.cloud); + //pcl::toROSMsg(*cloud, scene_srv.request.cloud); tf::poseEigenToMsg(Eigen::Affine3d(pose), scene_srv.request.robot_pose); sensor_msgs::CameraInfo info; info.K.at(0) = K(0, 0); From a964d96a4656c9c195cb84cfb2a545af46e6dea7 Mon Sep 17 00:00:00 2001 From: ferdianjovan Date: Fri, 25 Nov 2016 18:10:14 +0000 Subject: [PATCH 244/255] Fixed conversion --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 02fa525b..267cca91 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -256,9 +256,12 @@ reglib::RGBDFrame * getFrame(soma_llsd_msgs::Scene & scene){ cv::Mat rgb = rgb_ptr->image; cv_bridge::CvImagePtr depth_ptr; - try{ depth_ptr = cv_bridge::toCvCopy(scene.depth_img, sensor_msgs::image_encodings::MONO16);} + //try{ depth_ptr = cv_bridge::toCvCopy(scene.depth_img, sensor_msgs::image_encodings::MONO16);} + try{ depth_ptr = cv_bridge::toCvCopy(scene.depth_img, sensor_msgs::image_encodings::TYPE_32FC1);} catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat depth = depth_ptr->image; + //cv::Mat depth = depth_ptr->image; + cv::Mat depth; + depth_ptr->image.convertTo(depth, CV_16UC1, 1000.0); Eigen::Affine3d epose; tf::poseMsgToEigen(scene.robot_pose, epose); From 13e59ae2e49e756cfd67d99f8ce6c2a66d19da46 Mon Sep 17 00:00:00 2001 From: ferdianjovan Date: Sat, 26 Nov 2016 11:58:13 +0000 Subject: [PATCH 245/255] Changed to correct normalization --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 267cca91..2191a5a8 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -249,6 +249,7 @@ reglib::Camera * getCam(sensor_msgs::CameraInfo & info){ reglib::RGBDFrame * getFrame(soma_llsd_msgs::Scene & scene){ reglib::Camera * cam = getCam(scene.camera_info); + cam->idepth_scale = 1.0/5000.0; cv_bridge::CvImagePtr rgb_ptr; try{ rgb_ptr = cv_bridge::toCvCopy(scene.rgb_img, sensor_msgs::image_encodings::BGR8);} @@ -261,7 +262,7 @@ reglib::RGBDFrame * getFrame(soma_llsd_msgs::Scene & scene){ catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} //cv::Mat depth = depth_ptr->image; cv::Mat depth; - depth_ptr->image.convertTo(depth, CV_16UC1, 1000.0); + depth_ptr->image.convertTo(depth, CV_16UC1, 5000.0); Eigen::Affine3d epose; tf::poseMsgToEigen(scene.robot_pose, epose); From 21711ccce41f95a882d7270db220c640e29aa985 Mon Sep 17 00:00:00 2001 From: ferdianjovan Date: Sat, 26 Nov 2016 17:02:41 +0000 Subject: [PATCH 246/255] Testing configuration --- quasimodo/quasimodo_object_search/launch/object_search.launch | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_object_search/launch/object_search.launch b/quasimodo/quasimodo_object_search/launch/object_search.launch index b2da28b7..898a256c 100644 --- a/quasimodo/quasimodo_object_search/launch/object_search.launch +++ b/quasimodo/quasimodo_object_search/launch/object_search.launch @@ -1,9 +1,10 @@ - + + @@ -15,6 +16,7 @@ + From 32816c20d4c057c4861370cf0a1a23bd584c2182 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Sun, 27 Nov 2016 13:45:00 +0100 Subject: [PATCH 247/255] Fixed potential problem with querying and reloading --- .../src/quasimodo_retrieval_node.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index ba3f215b..ae4ceaab 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -112,6 +112,7 @@ class retrieval_node { double iss_model_resolution; // 0.004 double pfhrgb_radius_search; // 0.04 + bool is_running; void parameters_callback(quasimodo_retrieval::parametersConfig& config, uint32_t level) { iss_model_resolution = config.iss_model_resolution; @@ -162,6 +163,8 @@ class retrieval_node { vt.compute_normalizing_constants(); } + is_running = false; + //dynamic_reconfigure::Server::CallbackType f; //f = boost::bind(&retrieval_node::parameters_callback, this, _1, _2); @@ -187,11 +190,12 @@ class retrieval_node { { dynamic_object_retrieval::vocabulary_summary temp_summary; temp_summary.load(vocabulary_path); - if (summary.last_updated == temp_summary.last_updated) { + if (summary.last_updated == temp_summary.last_updated || is_running) { return false; } cout << "Re-loading new vocabulary with timestamp: " << temp_summary.last_updated << endl; + ros::Duration(0.2).sleep(); // sleep for a little bit to make sure the vocabulary is saved summary = temp_summary; vt = grouped_vocabulary_tree(); @@ -199,6 +203,8 @@ class retrieval_node { vt.set_cache_path(vocabulary_path.string()); vt.set_min_match_depth(3); vt.compute_normalizing_constants(); + + return true; } pair sweep_get_rgbd_at(const boost::filesystem::path& sweep_xml, int i) @@ -535,6 +541,7 @@ class retrieval_node { bool retrieval_implementation(const quasimodo_msgs::retrieval_query& query, quasimodo_msgs::retrieval_result& result, geometry_msgs::Transform& query_room_transform) { cout << "Received query msg of kind " << query.query_kind << "..." << endl; + is_running = true; dynamic_object_retrieval::uri_kind kind; switch (query.query_kind) { @@ -549,6 +556,7 @@ class retrieval_node { break; default: cout << "Invalid quasimodo_msgs::retrieval_query::query_kind option: " << query.query_kind << endl; + is_running = false; return false; } @@ -692,6 +700,8 @@ class retrieval_node { cout << "Finished retrieval..." << endl; + is_running = false; + return true; } From 7077a082fac95977b2f7063e78f0540be9a49732 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Sun, 27 Nov 2016 20:51:46 +0100 Subject: [PATCH 248/255] That was too short --- quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index ae4ceaab..88c8176b 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -195,7 +195,7 @@ class retrieval_node { } cout << "Re-loading new vocabulary with timestamp: " << temp_summary.last_updated << endl; - ros::Duration(0.2).sleep(); // sleep for a little bit to make sure the vocabulary is saved + ros::Duration(0.5).sleep(); // sleep for a little bit to make sure the vocabulary is saved summary = temp_summary; vt = grouped_vocabulary_tree(); From b92b94eb7d39ee0ff3ceb080ba28919fa013a05b Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 28 Nov 2016 01:03:19 +0100 Subject: [PATCH 249/255] Fixed some small stuff to make this work better with JOhan's code --- .../dynamic_retrieval.h | 8 --- .../launch/object_search.launch | 4 +- .../launch/retrieval.launch | 2 + .../src/quasimodo_retrieval_node.cpp | 50 +++++++++++++++---- 4 files changed, 45 insertions(+), 19 deletions(-) diff --git a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h index 3dbb1540..2ee734dd 100644 --- a/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h +++ b/dynamic_object_retrieval/dynamic_object_retrieval/include/dynamic_object_retrieval/dynamic_retrieval.h @@ -76,10 +76,6 @@ std::vector get_retrieved_paths(const std::vector= offset) { std::cout << "1. Index: " << s.index << " is larger than: " << offset << std::endl; @@ -272,10 +268,6 @@ get_retrieved_path_scores(const std::vector::r uris.load(boost::filesystem::path(summary.noise_data_path) / "vocabulary"); mongodb_store::MessageStoreProxy* message_store = NULL; - for (IndexT s : scores) { - std::cout << "1. Index: " << s.subgroup_global_indices[0] << std::endl; - } - for (IndexT s : scores) { int index = s.subgroup_global_indices[0]; if (index >= offset) { diff --git a/quasimodo/quasimodo_object_search/launch/object_search.launch b/quasimodo/quasimodo_object_search/launch/object_search.launch index b2da28b7..356ca69d 100644 --- a/quasimodo/quasimodo_object_search/launch/object_search.launch +++ b/quasimodo/quasimodo_object_search/launch/object_search.launch @@ -1,14 +1,16 @@ - + + + diff --git a/quasimodo/quasimodo_retrieval/launch/retrieval.launch b/quasimodo/quasimodo_retrieval/launch/retrieval.launch index 086ba2e2..69b970b6 100644 --- a/quasimodo/quasimodo_retrieval/launch/retrieval.launch +++ b/quasimodo/quasimodo_retrieval/launch/retrieval.launch @@ -2,11 +2,13 @@ + + diff --git a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp index 88c8176b..dd34da72 100644 --- a/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp +++ b/quasimodo/quasimodo_retrieval/src/quasimodo_retrieval_node.cpp @@ -93,6 +93,7 @@ class retrieval_node { ros::NodeHandle n; ros::Publisher pub; ros::Publisher img_pub; + ros::Publisher depth_pub; ros::ServiceServer service; ros::Publisher keypoint_pub; ros::Publisher pubs[10]; @@ -114,6 +115,8 @@ class retrieval_node { double pfhrgb_radius_search; // 0.04 bool is_running; + int min_match_depth; + void parameters_callback(quasimodo_retrieval::parametersConfig& config, uint32_t level) { iss_model_resolution = config.iss_model_resolution; pfhrgb_radius_search = config.pfhrgb_radius_search; @@ -141,6 +144,8 @@ class retrieval_node { pn.param("output", retrieval_output, std::string("retrieval_result")); pn.param("input", retrieval_input, std::string("/models/query")); + pn.param("min_match_depth", min_match_depth, 3); + summary.load(vocabulary_path); cout << "Loaded summary from: " << (vocabulary_path / "vocabulary_summary.json").string() << endl; cout << "With data path: " << summary.noise_data_path << endl; @@ -159,7 +164,7 @@ class retrieval_node { if (vt.empty()) { dynamic_object_retrieval::load_vocabulary(vt, vocabulary_path); vt.set_cache_path(vocabulary_path.string()); - vt.set_min_match_depth(3); + vt.set_min_match_depth(min_match_depth); vt.compute_normalizing_constants(); } @@ -171,7 +176,8 @@ class retrieval_node { server.setCallback(boost::bind(&retrieval_node::parameters_callback, this, _1, _2)); pub = n.advertise(retrieval_output, 1); - img_pub = n.advertise("/quasimodo_retrieval/raw_visualization", 1, true); + img_pub = n.advertise("/quasimodo_retrieval/raw_rgb_visualization", 1, true); + depth_pub = n.advertise("/quasimodo_retrieval/raw_depth_visualization", 1, true); keypoint_pub = n.advertise("/models/keypoints", 1); for (size_t i = 0; i < 10; ++i) { pubs[i] = n.advertise(string("/retrieval_cloud/") + to_string(i), 1, true); @@ -195,13 +201,13 @@ class retrieval_node { } cout << "Re-loading new vocabulary with timestamp: " << temp_summary.last_updated << endl; - ros::Duration(0.5).sleep(); // sleep for a little bit to make sure the vocabulary is saved + ros::Duration(3.0).sleep(); // sleep for a little bit to make sure the vocabulary is saved summary = temp_summary; vt = grouped_vocabulary_tree(); dynamic_object_retrieval::load_vocabulary(vt, vocabulary_path); vt.set_cache_path(vocabulary_path.string()); - vt.set_min_match_depth(3); + vt.set_min_match_depth(min_match_depth); vt.compute_normalizing_constants(); return true; @@ -602,7 +608,7 @@ class retrieval_node { * * */ - auto results = dynamic_object_retrieval::query_reweight_vocabulary((VocabularyT&)vt, features, 200, vocabulary_path, summary, true, kind); + auto results = dynamic_object_retrieval::query_reweight_vocabulary((VocabularyT&)vt, features, 200, vocabulary_path, summary, false, kind); // This is just to make sure that we have valid results even when some meta rooms have been deleted prune_results(results); @@ -688,15 +694,23 @@ class retrieval_node { tf::transformTFToMsg(room_transform, query_room_transform); result = construct_msgs(retrieved_clouds, initial_poses, images, depths, masks, paths, scores, indices, vocabulary_ids, global_transforms); + + cout << "============ Returning filtered results: ============" << endl; for (int i = 0; i < retrieved_clouds.size(); ++i) { sensor_msgs::PointCloud2 cloud_msg = result.retrieved_clouds[i]; cloud_msg.header.stamp = ros::Time::now(); cloud_msg.header.frame_id = "/map"; pubs[i].publish(cloud_msg); + + cout << "Path: " << result.retrieved_image_paths[i].strings[0] << ", Voc. ind: " << result.vocabulary_ids[i] << endl; } + cout << "=====================================================" << endl; - sensor_msgs::Image img_msg = construct_results_image(images); + sensor_msgs::Image img_msg; + sensor_msgs::Image depth_msg; + tie(img_msg, depth_msg) = construct_results_image(images, depths); img_pub.publish(img_msg); + depth_pub.publish(depth_msg); cout << "Finished retrieval..." << endl; @@ -709,7 +723,16 @@ class retrieval_node { quasimodo_msgs::query_cloud::Response& res) { geometry_msgs::Transform query_room_transform; - return retrieval_implementation(req.query, res.result, query_room_transform); + + bool success = retrieval_implementation(req.query, res.result, query_room_transform); + + quasimodo_msgs::retrieval_query_result result; + result.query = req.query; + result.result = res.result; + result.query.room_transform = query_room_transform; + pub.publish(result); + + return success; } void run_retrieval(const quasimodo_msgs::retrieval_query::ConstPtr& query_msg) //const string& sweep_xml) @@ -746,7 +769,8 @@ class retrieval_node { ros_image = *cv_pub_ptr->toImageMsg(); } - sensor_msgs::Image construct_results_image(const vector >& images) + pair construct_results_image(const vector >& images, + const vector >& depths) { int width = 640; int height = 480; @@ -756,18 +780,24 @@ class retrieval_node { cout << "Images size: " << images.size() << endl; cv::Mat visualization = cv::Mat::zeros(height*sizes.first, width*sizes.second, CV_8UC3); + cv::Mat depth_visualization = cv::Mat::zeros(height*sizes.first, width*sizes.second, CV_16UC1); for (size_t i = 0; i < images.size(); ++i) { cout << "Image " << i << " size: " << images[i].size() << endl; cout << "Image " << i << " dimensions: " << images[i][0].rows << " x " << images[i][0].cols << endl; size_t offset_height = i / sizes.second; size_t offset_width = i % sizes.second; - images[i][0].copyTo(visualization(cv::Rect(offset_width*width, offset_height*height, width, height))); + cv::Rect image_rect(offset_width*width, offset_height*height, width, height); + images[i][0].copyTo(visualization(image_rect)); + depths[i][0].copyTo(depth_visualization(image_rect)); } sensor_msgs::Image img_msg; convert_to_img_msg(visualization, img_msg); - return img_msg; + sensor_msgs::Image depth_msg; + convert_to_depth_msg(depth_visualization, depth_msg); + + return make_pair(img_msg, depth_msg); } quasimodo_msgs::retrieval_result construct_msgs(const vector& clouds, From 3706bf4e9044e29bf52f5f18716ce826b005320b Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 28 Nov 2016 01:04:20 +0100 Subject: [PATCH 250/255] Added a publisher in the data visualizer --- .../src/visualize_database_node.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/quasimodo/quasimodo_object_search/src/visualize_database_node.cpp b/quasimodo/quasimodo_object_search/src/visualize_database_node.cpp index 349843ec..8ff842aa 100644 --- a/quasimodo/quasimodo_object_search/src/visualize_database_node.cpp +++ b/quasimodo/quasimodo_object_search/src/visualize_database_node.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include @@ -14,12 +15,15 @@ class visualization_server { ros::NodeHandle n; mongodb_store::MessageStoreProxy db_client; + ros::Publisher pub; ros::Subscriber sub; boost::shared_ptr viewer; size_t counter; visualization_server() : db_client(n, "quasimodo", "world_state"), viewer(new pcl::visualization::PCLVisualizer("3D Viewer")), counter(0) { + pub = n.advertise("/model/retrieval_db", 1); + sub = n.subscribe("/model/added_to_db", 1, &visualization_server::callback, this); viewer->setBackgroundColor(1, 1, 1); @@ -35,6 +39,7 @@ class visualization_server { viewer->removeAllPointClouds(); size_t start_counter = counter; + CloudT::Ptr combined(new CloudT); for (boost::shared_ptr& msg : messages) { if (!msg->removed_at.empty()) { continue; @@ -45,6 +50,7 @@ class visualization_server { cout << "Adding " << counter << ":th point cloud..." << endl; CloudT::Ptr cloud(new CloudT); pcl::fromROSMsg(msg->surfel_cloud, *cloud); + *combined += *cloud; float offset = 1.0f*float(counter - start_counter); for (PointT& p : cloud->points) { @@ -57,6 +63,10 @@ class visualization_server { viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, cloud_name); ++counter; } + + sensor_msgs::PointCloud2 msg_cloud; + pcl::toROSMsg(*combined, msg_cloud); + pub.publish(msg_cloud); } void run() From 2417d538abc806d4f8ef8ed8b67926582dc61847 Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Mon, 28 Nov 2016 13:07:11 +0100 Subject: [PATCH 251/255] Added a couple of packages to make starting work --- quasimodo/quasimodo_brain/launch/modelserver.launch | 7 +------ quasimodo/quasimodo_brain/launch/segmentation.launch | 4 ++++ 2 files changed, 5 insertions(+), 6 deletions(-) create mode 100644 quasimodo/quasimodo_brain/launch/segmentation.launch diff --git a/quasimodo/quasimodo_brain/launch/modelserver.launch b/quasimodo/quasimodo_brain/launch/modelserver.launch index 23cf19d1..514bd62d 100644 --- a/quasimodo/quasimodo_brain/launch/modelserver.launch +++ b/quasimodo/quasimodo_brain/launch/modelserver.launch @@ -1,8 +1,3 @@ - - - - - - + diff --git a/quasimodo/quasimodo_brain/launch/segmentation.launch b/quasimodo/quasimodo_brain/launch/segmentation.launch new file mode 100644 index 00000000..a7584fef --- /dev/null +++ b/quasimodo/quasimodo_brain/launch/segmentation.launch @@ -0,0 +1,4 @@ + + + + From 84845c8be5431163cb5e6d87c506e8bee9cb012e Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Mon, 28 Nov 2016 16:48:13 +0100 Subject: [PATCH 252/255] ... --- quasimodo/quasimodo_brain/src/Util/Util.cpp | 158 +-- quasimodo/quasimodo_brain/src/Util/Util.h | 2 +- .../metaroom_additional_view_processing.cpp | 47 +- quasimodo/quasimodo_brain/src/modelserver.cpp | 975 ++---------------- .../quasimodo_models/include/model/Model.h | 1 + .../registration/.MassRegistrationPPR2.h.swp | Bin 0 -> 16384 bytes .../quasimodo_models/src/model/Model.cpp | 5 +- .../modelupdater/ModelUpdaterBasicFuse.cpp | 6 - quasimodo/quasimodo_msgs/CMakeLists.txt | 1 + quasimodo/quasimodo_msgs/srv/recognize.srv | 3 + 10 files changed, 208 insertions(+), 990 deletions(-) create mode 100644 quasimodo/quasimodo_models/include/registration/.MassRegistrationPPR2.h.swp create mode 100644 quasimodo/quasimodo_msgs/srv/recognize.srv diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 02fa525b..040907cd 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -2,15 +2,20 @@ namespace quasimodo_brain { -//Save? -//std::vector rep_relativeposes; -//std::vector rep_frames; -//std::vector rep_modelmasks; +int getdir (std::string dir, std::vector & files){ + DIR *dp; + struct dirent *dirp; + if((dp = opendir(dir.c_str())) == NULL) { + cout << "Error(" << errno << ") opening " << dir << endl; + return errno; + } -//std::vector > scores; -//std::vector submodels; -//std::vector submodels_relativeposes; -//std::vector > submodels_scores; + while ((dirp = readdir(dp)) != NULL) { + files.push_back(std::string(dirp->d_name)); + } + closedir(dp); + return 0; +} std::string getPoseString(Eigen::Matrix4d pose){ char buf [1024]; @@ -102,21 +107,59 @@ std::string initSegment(ros::NodeHandle& n, reglib::Model * model){ reglib::Model * getModelFromSegment(ros::NodeHandle& n, std::string segment_id){ reglib::Model * model = new reglib::Model(); - ros::ServiceClient client = n.serviceClient("/soma_llsd/get_segment"); + ros::ServiceClient segclient = n.serviceClient("/soma_llsd/get_segment"); ROS_INFO("Waiting for /soma_llsd/get_segment service..."); - if (!client.waitForExistence(ros::Duration(1.0))) { + if (!segclient.waitForExistence(ros::Duration(1.0))) { ROS_INFO("Failed to get /soma_llsd/get_segment service!"); return model; } ROS_INFO("Got /soma_llsd/get_segment service"); - soma_llsd::GetSegment srv; - srv.request.segment_id = model->soma_id; - if (client.call(srv)) { + soma_llsd::GetSegment segsrv; + segsrv.request.segment_id = model->soma_id; + if (segclient.call(segsrv)) { ROS_INFO("Got /soma_llsd/get_segment"); return model; } + ros::ServiceClient client = n.serviceClient("/soma_llsd/get_scene"); + ROS_INFO("Waiting for /soma_llsd/get_scene service..."); + if (!client.waitForExistence(ros::Duration(1.0))) { + ROS_INFO("Failed to get /soma_llsd/get_scene service!"); + return model; + } + ROS_INFO("Got /soma_llsd/get_scene service"); + + soma_llsd_msgs::Segment segment = segsrv.response.response; + for (const soma_llsd_msgs::Observation& obs : segment.observations) { + soma_llsd::GetScene srv; + srv.request.scene_id = obs.scene_id; + if (!client.call(srv)) { + ROS_ERROR("Failed to call service /soma_llsd/get_scene"); + return model; + } + + cv::Mat mask; + + sensor_msgs::Image image_mask = obs.image_mask; + reglib::RGBDFrame * frame = getFrame(srv.response.response); + + model->frames.push_back(frame); + + geometry_msgs::Pose pose1 = obs.pose; + Eigen::Affine3d epose1; + tf::poseMsgToEigen(pose1, epose1); + model->relativeposes.push_back(epose1.matrix()); + +// cv_bridge::CvImagePtr mask_ptr; +// try{ mask_ptr = cv_bridge::toCvCopy(msg.masks[i], sensor_msgs::image_encodings::MONO8);} +// catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} +// cv::Mat mask = mask_ptr->image; + quasimodo_conversions::convert_to_mask_msg(mask,image_mask); + model->modelmasks.push_back(new reglib::ModelMask(mask)); + } + model->recomputeModelPoints(); + //Read frames etc //Read parameters //Read submodels @@ -256,9 +299,12 @@ reglib::RGBDFrame * getFrame(soma_llsd_msgs::Scene & scene){ cv::Mat rgb = rgb_ptr->image; cv_bridge::CvImagePtr depth_ptr; - try{ depth_ptr = cv_bridge::toCvCopy(scene.depth_img, sensor_msgs::image_encodings::MONO16);} + //try{ depth_ptr = cv_bridge::toCvCopy(scene.depth_img, sensor_msgs::image_encodings::MONO16);} + try{ depth_ptr = cv_bridge::toCvCopy(scene.depth_img, sensor_msgs::image_encodings::TYPE_32FC1);} catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - cv::Mat depth = depth_ptr->image; + //cv::Mat depth = depth_ptr->image; + cv::Mat depth; + depth_ptr->image.convertTo(depth, CV_16UC1, 1000.0); Eigen::Affine3d epose; tf::poseMsgToEigen(scene.robot_pose, epose); @@ -1295,8 +1341,7 @@ Eigen::Matrix4f getRegisteredViewPoses(const std::string& poses_file){ cout<<"ERROR: cannot find poses file "< getIndsFromFile(const std::string& poses_file){ cout<<"ERROR: cannot find idnex file "< getIndsFromFile(const std::string& poses_file){ std::vector loadModelsPCDs(std::string path){ + +// boost::shared_ptr viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("Modelserver Viewer")); +// viewer->addCoordinateSystem(0.1); +// viewer->setBackgroundColor(1.0,0.0,1.0); + + std::vector models; std::vector folders = getFolderList(path);//recursiveGetFolderList(path);//getFolderList(path);// @@ -1354,7 +1402,6 @@ std::vector loadModelsPCDs(std::string path){ if (files[j].find("pose") != std::string::npos){posepaths.push_back(file);} } - printf("%i %i %i\n",cloudspaths.size(),indexpaths.size(),posepaths.size()); if((cloudspaths.size() == indexpaths.size()) && (cloudspaths.size() == posepaths.size())){ std::sort (cloudspaths.begin(), cloudspaths.end()); std::sort (indexpaths.begin(), indexpaths.end()); @@ -1362,48 +1409,23 @@ std::vector loadModelsPCDs(std::string path){ reglib::Model * model = new reglib::Model(); - - // for(unsigned int i = 0; i < msg.local_poses.size(); i++){ - // sensor_msgs::CameraInfo camera = msg.frames[i].camera; - // ros::Time capture_time = msg.frames[i].capture_time; - // geometry_msgs::Pose pose = msg.frames[i].pose; - - // cv_bridge::CvImagePtr rgb_ptr; - // try{ rgb_ptr = cv_bridge::toCvCopy(msg.frames[i].rgb, sensor_msgs::image_encodings::BGR8);} - // catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - // cv::Mat rgb = rgb_ptr->image; - - // cv_bridge::CvImagePtr depth_ptr; - // try{ depth_ptr = cv_bridge::toCvCopy(msg.frames[i].depth, sensor_msgs::image_encodings::MONO16);} - // catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());} - // cv::Mat depth = depth_ptr->image; - - // Eigen::Affine3d epose; - // tf::poseMsgToEigen(pose, epose); - - // reglib::Camera * cam = new reglib::Camera(); - // if(camera.K[0] > 0){ - // cam->fx = camera.K[0]; - // cam->fy = camera.K[4]; - // cam->cx = camera.K[2]; - // cam->cy = camera.K[5]; - // } - - // } - // - for(unsigned int j = 0; j < cloudspaths.size(); j++){ std::string cloudpath = cloudspaths[j]; std::string indexpath = indexpaths[j]; std::string posepath = posepaths[j]; - printf("loading: %s %s %s\n", cloudpath.c_str(),indexpath.c_str(),posepath.c_str()); + //printf("loading: %s %s %s\n", cloudpath.c_str(),indexpath.c_str(),posepath.c_str()); pcl::PointCloud::Ptr cloud (new pcl::PointCloud); if (pcl::io::loadPCDFile (cloudpath, *cloud) != -1){ + +// viewer->removeAllPointClouds(); +// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); +// viewer->spin(); + Eigen::Matrix4f pose = getRegisteredViewPoses(posepath); std::vector indexes = getIndsFromFile(indexpath); - std::cout << pose << std::endl << std::endl; - std::cout << indexes.size() << std::endl; +// std::cout << pose << std::endl << std::endl; +// std::cout << indexes.size() << std::endl; unsigned int width = cloud->width; unsigned int height = cloud->height; @@ -1416,16 +1438,8 @@ std::vector loadModelsPCDs(std::string path){ unsigned short * depthdata = (unsigned short *)(depth.data); reglib::Camera * cam = new reglib::Camera();//figure out cam params - - //TODO figure out optical centre and focal lengths from cloud... - // for(unsigned int w = 0; w < width; w++){ - // for(unsigned int w = 0; w < width; w++){ - // pcl::PointXYZRGB p = cloud->points[k]; - // float x = p.x; - // float y = p.y; - // float z = p.z; - // } - // } + cam->fx = 525; + cam->fy = 525; for(unsigned int k = 0; k < width*height; k++){ pcl::PointXYZRGB p = cloud->points[k]; @@ -1439,22 +1453,28 @@ std::vector loadModelsPCDs(std::string path){ } } - cv::Mat mask; mask.create(height,width,CV_8UC1); unsigned char * maskdata = mask.data; + + for(unsigned int k = 0; k < height*width; k++){maskdata[k] = 0;} for(unsigned int k = 0; k < indexes.size(); k++){maskdata[indexes[k]] = 255;} + +// cv::imshow( "mask", mask ); +// cv::waitKey(0); + reglib::RGBDFrame * frame = new reglib::RGBDFrame(cam,rgb, depth, 0 , pose.cast() , true,"",false); + frame->show(false); model->frames.push_back(frame); model->relativeposes.push_back(model->frames.front()->pose.inverse() * pose.cast()); model->modelmasks.push_back(new reglib::ModelMask(mask)); + model->modelmasks.back()->label = folders[i]; }else{ printf ("Couldn't read pcd file\n"); } } - - model->recomputeModelPoints(); + model->recomputeModelPoints();//model->recomputeModelPoints(Eigen::Matrix4d::Identity(),viewer); models.push_back(model); }else{ printf("number of clouds, indices and poses dont match... ignoring\n"); diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index 8b6ef659..ae708fd7 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -44,7 +44,7 @@ #include namespace quasimodo_brain { - +int getdir (std::string dir, std::vector & files); std::string getPoseString(Eigen::Matrix4d pose); Eigen::Matrix4d getPoseFromString(std::string str); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 7dd41d9d..efe224a1 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -1,25 +1,20 @@ #include "ros/ros.h" #include "std_msgs/String.h" -#include #include #include -#include #include "eigen_conversions/eigen_msg.h" #include "tf_conversions/tf_eigen.h" #include "metaroom_xml_parser/simple_xml_parser.h" #include "metaroom_xml_parser/simple_summary_parser.h" - #include -#include -#include "metaroom_xml_parser/load_utilities.h" +#include #include #include - #include "quasimodo_msgs/model.h" #include "quasimodo_msgs/rgbd_frame.h" #include "quasimodo_msgs/model_from_frame.h" @@ -28,41 +23,34 @@ #include "quasimodo_msgs/get_model.h" #include "quasimodo_msgs/segment_model.h" #include "quasimodo_msgs/metaroom_pair.h" - #include #include #include + +#include +#include #include #include "modelupdater/ModelUpdater.h" #include "core/RGBDFrame.h" #include "Util/Util.h" -// Services -#include -#include -#include - -// Registration service #include -// Additional view mask service -#include - - #include #include -// Custom messages #include "object_manager/dynamic_object.h" #include "object_manager/dynamic_object_xml_parser.h" #include "object_manager/dynamic_object_utilities.h" #include "object_manager/dynamic_object_mongodb_interface.h" - #include #include - +#include +#include +#include +#include #include @@ -748,20 +736,21 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: } //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); */ -// double startTime_store = reglib::getTime(); -// fullmodel->pointspath = sweep_folder+"/fullmodel_superpoints.bin"; -// std::string soma_id = quasimodo_brain::initSegment(*np,fullmodel); -// printf("soma_id: %i\n",soma_id.c_str()); -// printf("add time: %6.6f\n",reglib::getTime()-startTime_store); + double startTime_store = reglib::getTime(); + fullmodel->pointspath = sweep_folder+"/fullmodel_superpoints.bin"; + std::string soma_id = quasimodo_brain::initSegment(*np,fullmodel); + printf("soma_id: %i\n",soma_id.c_str()); + printf("add time: %6.6f\n",reglib::getTime()-startTime_store); -// startTime_store = reglib::getTime(); +// // std::string soma_id2 = quasimodo_brain::initSegment(*np,fullmodel); // printf("soma_id2: %i\n",soma_id2.c_str()); // printf("add2 time: %6.6f\n",reglib::getTime()-startTime_store); -// reglib::Model * test = quasimodo_brain::getModelFromSegment(*np,soma_id); -// printf("load time: %6.6f\n",reglib::getTime()-startTime_store); -//exit(0); + startTime_store = reglib::getTime(); + reglib::Model * test = quasimodo_brain::getModelFromSegment(*np,soma_id); + printf("load time: %6.6f\n",reglib::getTime()-startTime_store); +exit(0); return fullmodel; } diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 2160c2ba..2980fba4 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -15,12 +15,11 @@ #include "quasimodo_msgs/retrieval_query_result.h" #include "quasimodo_msgs/retrieval_query.h" #include "quasimodo_msgs/retrieval_result.h" +#include "quasimodo_msgs/recognize.h" #include "soma_msgs/SOMAObject.h" #include "soma_manager/SOMAInsertObjs.h" -//#include "modelupdater/ModelUpdater.h" -//#include "/home/johane/catkin_ws_dyn/src/quasimodo_models/include/modelupdater/ModelUpdater.h" #include "modelupdater/ModelUpdater.h" #include "core/RGBDFrame.h" #include @@ -34,15 +33,10 @@ #include #include #include - #include - #include "ModelDatabase/ModelDatabase.h" - #include - #include - #include #include #include @@ -52,6 +46,10 @@ #include "Util/Util.h" +bool addToDB(ModelDatabase * database, reglib::Model * model, bool add); +bool addIfPossible(ModelDatabase * database, reglib::Model * model, reglib::Model * model2); +void addNewModel(reglib::Model * model); + using namespace quasimodo_brain; bool visualization = false; @@ -76,7 +74,7 @@ std::map updaters; std::set framekeys; reglib::RegistrationRandom * registration; ModelDatabase * modeldatabase; - +ros::NodeHandle * nh; std::string savepath = "."; boost::shared_ptr viewer; @@ -84,7 +82,6 @@ boost::shared_ptr viewer; ros::Publisher models_new_pub; ros::Publisher models_updated_pub; ros::Publisher models_deleted_pub; - ros::Publisher model_pcd_pub; ros::Publisher database_pcd_pub; ros::Publisher model_history_pub; @@ -92,25 +89,16 @@ ros::Publisher model_places_pub; ros::Publisher model_last_pub; ros::Publisher chatter_pub; -//<<<<<<< HEAD ros::ServiceClient retrieval_client; ros::ServiceClient conversion_client; ros::ServiceClient insert_client; -//ros::ServiceClient soma2add; -////======= -//ros::ServiceClient soma_add; -//>>>>>>> integrate_object_search - -double occlusion_penalty = 10; -double massreg_timeout = 120; - -bool run_search = false; -double search_timeout = 30; - -int sweepid_counter = 0; - -int current_model_update = 0; +double occlusion_penalty = 10; +double massreg_timeout = 120; +bool run_search = false; +double search_timeout = 30; +int sweepid_counter = 0; +int current_model_update = 0; bool myfunction (reglib::Model * i,reglib::Model * j) { return (i->frames.size() + i->submodels.size()) > (j->frames.size() + j->submodels.size()); } @@ -147,11 +135,9 @@ void publishDatabasePCD(bool original_colors = false){ } float minx = 100000000000; - for(unsigned int j = 0; j < cloud->points.size(); j++){minx = std::min(cloud->points[j].x , minx);} for(unsigned int j = 0; j < cloud->points.size(); j++){cloud->points[j].x += maxx-minx + 0.15;} for(unsigned int j = 0; j < cloud->points.size(); j++){maxx = std::max(cloud->points[j].x,maxx);} - for(unsigned int j = 0; j < cloud->points.size(); j++){conccloud->points.push_back(cloud->points[j]);} } @@ -188,16 +174,14 @@ void dumpDatabase(std::string path = "."){ } void retrievalCallback(const quasimodo_msgs::retrieval_query_result & qr){ - printf("retrievalCallback\n"); - sresult = qr.result; - new_search_result = true; - last_search_result_time = getTime(); + printf("retrievalCallback\n"); + sresult = qr.result; + new_search_result = true; + last_search_result_time = getTime(); } void showModels(std::vector mods){ - //viewer->removeAllPointClouds(); float maxx = 0; - pcl::PointCloud::Ptr conccloud (new pcl::PointCloud); for(unsigned int i = 0; i < mods.size(); i++){ pcl::PointCloud::Ptr cloud = mods[i]->getPCLcloud(1, false); @@ -224,11 +208,6 @@ void showModels(std::vector mods){ for(unsigned int j = 0; j < cloud->points.size(); j++){minx = std::min(cloud->points[j].x , minx);} for(unsigned int j = 0; j < cloud->points.size(); j++){cloud->points[j].x += maxx-minx + 0.15;} for(unsigned int j = 0; j < cloud->points.size(); j++){maxx = std::max(cloud->points[j].x,maxx);} - -// char buf [1024]; -// sprintf(buf,"cloud%i",i); -// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), buf); - *conccloud += *cloud; } @@ -245,9 +224,6 @@ void showModels(std::vector mods){ viewer->addPointCloud (conccloud, pcl::visualization::PointCloudColorHandlerRGBField(conccloud), "conccloud"); viewer->spin(); } - -// printf("show_sorted()\n"); -// exit(0); } int savecounter = 0; @@ -260,132 +236,25 @@ void show_sorted(){ showModels(results); } -std::vector getSOMA2ObjectMSGs(reglib::Model * model){ - std::vector msgs; - for(unsigned int i = 0; i < model->frames.size(); i++){ - soma_msgs::SOMAObject msg; - char buf [1024]; - sprintf(buf,"id_%i_%i",int(model->id),int(model->frames[i]->id)); - msg.id = std::string(buf); - msg.map_name = "whatevermapname"; // #### the global map that the object belongs. Automatically filled by SOMA2 insert service - msg.map_unique_id = ""; // #### the unique db id of the map. Automatically filled by SOMA2 insert service - msg.config = "configid"; // #### the corresponding world configuration. This could be incremented as config1, config2 at each meta-room observation - msg.mesh = ""; // #### mesh model of the object. Could be left blank - sprintf(buf,"type_%i",int(model->id)); - msg.type = std::string(buf); // #### type of the object. For higher level objects, this could chair1, chair2. For middle or lower level segments it could be segment1101, segment1102, etc. - // msg.waypoint = ""; // #### the waypoint id. Could be left blank - msg.timestep = 0; // #### this is the discrete observation instance. This could be incremented at each meta-room observation as 1,2,3,etc... - msg.logtimestamp = 1000000.0 * double(model->frames[i]->capturetime); // #### this is the unix timestamp information that holds the time when the object is logged. SOMA2 uses UTC time values. - - Eigen::Affine3f transform = Eigen::Affine3f(model->frames[i]->pose.cast());//Eigen::Affine3f::Identity(); - - bool * maskdata = model->modelmasks[i]->maskvec; - unsigned char * rgbdata = (unsigned char *) model->frames[i]->rgb.data; - unsigned short * depthdata = (unsigned short *) model->frames[i]->depth.data; - - const unsigned int width = model->frames[i]->camera->width; const unsigned int height = model->frames[i]->camera->height; - const double idepth = model->frames[i]->camera->idepth_scale; - const double cx = model->frames[i]->camera->cx; const double cy = model->frames[i]->camera->cy; - const double ifx = 1.0/model->frames[i]->camera->fx; const double ify = 1.0/model->frames[i]->camera->fy; - - pcl::PointCloud::Ptr cloud (new pcl::PointCloud); - pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud); - for(unsigned int w = 0; w < width; w++){ - for(unsigned int h = 0; h < height;h++){ - int ind = h*width+w; - double z = idepth*double(depthdata[ind]); - if(z > 0 && maskdata[ind]){ - pcl::PointXYZRGB p; - p.x = (double(w) - cx) * z * ifx; - p.y = (double(h) - cy) * z * ify; - p.z = z; - p.b = rgbdata[3*ind+0]; - p.g = rgbdata[3*ind+1]; - p.r = rgbdata[3*ind+2]; - cloud->points.push_back(p); - } - } - } - - pcl::transformPointCloud (*cloud, *transformed_cloud, transform); - sensor_msgs::PointCloud2ConstPtr input; - pcl::fromROSMsg (*input, *transformed_cloud); - - - msg.cloud = *input; //#### The 3D cloud of the object (with respect to /map frame) - - Eigen::Quaterniond q = Eigen::Quaterniond(Eigen::Affine3d(model->frames[i]->pose).rotation()); - - geometry_msgs::Pose pose; - pose.orientation.x = q.x(); - pose.orientation.y = q.y(); - pose.orientation.z = q.z(); - pose.orientation.w = q.w(); - pose.position.x = model->frames[i]->pose(0,3); - pose.position.y = model->frames[i]->pose(1,3); - pose.position.z = model->frames[i]->pose(2,3); - msg.pose = pose; // #### Object pose in 3D (with respect to /map frame) - // geometry_msgs/Pose sweepCenter #### The pose of the robot during sweep (with respect to /map frame) - msgs.push_back(msg); - } - - // msg.model_id = model->id; - // msg.local_poses.resize(model->relativeposes.size()); - // msg.frames.resize(model->frames.size()); - // msg.masks.resize(model->modelmasks.size()); - // for(unsigned int i = 0; i < model->relativeposes.size(); i++){ - // geometry_msgs::Pose pose1; - // tf::poseEigenToMsg (Eigen::Affine3d(model->relativeposes[i]), pose1); - // geometry_msgs::Pose pose2; - // tf::poseEigenToMsg (Eigen::Affine3d(model->frames[i]->pose), pose2); - // cv_bridge::CvImage rgbBridgeImage; - // rgbBridgeImage.image = model->frames[i]->rgb; - // rgbBridgeImage.encoding = "bgr8"; - // cv_bridge::CvImage depthBridgeImage; - // depthBridgeImage.image = model->frames[i]->depth; - // depthBridgeImage.encoding = "mono16"; - // cv_bridge::CvImage maskBridgeImage; - // maskBridgeImage.image = model->modelmasks[i]->getMask(); - // maskBridgeImage.encoding = "mono8"; - // msg.local_poses[i] = pose1; - // msg.frames[i].capture_time = ros::Time(); - // msg.frames[i].pose = pose2; - // msg.frames[i].frame_id = model->frames[i]->id; - // msg.frames[i].rgb = *(rgbBridgeImage.toImageMsg()); - // msg.frames[i].depth = *(depthBridgeImage.toImageMsg()); - // msg.masks[i] = *(maskBridgeImage.toImageMsg());//getMask() - // } - return msgs; -} -void publishModel(reglib::Model * model){ -//<<<<<<< HEAD -// soma_manager::SOMA2InsertObjs objs; -// objs.request.objects = getSOMA2ObjectMSGs(model); -// if (soma2add.call(objs)){ -// }else{ROS_ERROR("Failed to call service soma2add");} -//======= -// // std::vector somamsgs = getSOMA2ObjectMSGs(model); -// soma_manager::SOMAInsertObjs objs; -// objs.request.objects = getSOMA2ObjectMSGs(model); -// if (soma_add.call(objs)){//Add frame to model server -// //// int frame_id = ifsrv.response.frame_id; -// }else{ROS_ERROR("Failed to call service soma_add");} - -// // for(int i = 0; i < somamsgs.size(); i++){ -// // if (soma_add.call(somamsgs[i])){//Add frame to model server -// //// int frame_id = ifsrv.response.frame_id; -// // }else{ROS_ERROR("Failed to call service soma_add");} -// // } -//>>>>>>> integrate_object_search +bool getModel(quasimodo_msgs::get_model::Request & req, quasimodo_msgs::get_model::Response & res){ + int model_id = req.model_id; + reglib::Model * model = models[model_id]; + res.model = getModelMSG(model); + return true; } - -bool getModel(quasimodo_msgs::get_model::Request & req, quasimodo_msgs::get_model::Response & res){ - int model_id = req.model_id; - reglib::Model * model = models[model_id]; - res.model = getModelMSG(model); - return true; +bool recognizeService(quasimodo_msgs::recognize::Request & req, quasimodo_msgs::recognize::Response & res){ + reglib::Model * mod = quasimodo_brain::getModelFromSegment(*nh, req.id); + addNewModel(mod); + reglib::Model * p = mod->parrent; + if((p == 0) || ((p->frames.size() == 0) && (p->submodels.size() == 1))){ + req.id = ""; + return false; + }else{ + req.id = p->soma_id; + return true; + } } bool indexFrame(quasimodo_msgs::index_frame::Request & req, quasimodo_msgs::index_frame::Response & res){ @@ -418,11 +287,7 @@ bool indexFrame(quasimodo_msgs::index_frame::Request & req, quasimodo_msgs::ind return true; } -bool addToDB(ModelDatabase * database, reglib::Model * model, bool add);//, bool deleteIfFail = false); bool addIfPossible(ModelDatabase * database, reglib::Model * model, reglib::Model * model2){ - - printf("addIfPossible\n"); - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model2, reg); mu->occlusion_penalty = occlusion_penalty; @@ -434,7 +299,6 @@ bool addIfPossible(ModelDatabase * database, reglib::Model * model, reglib::Mode reg->visualizationLvl = show_reg_lvl; reglib::FusionResults fr = mu->registerModel(model); - if(fr.score > 100){ reglib::UpdatedModels ud = mu->fuseData(&fr, model2, model); delete mu; @@ -445,20 +309,12 @@ bool addIfPossible(ModelDatabase * database, reglib::Model * model, reglib::Mode database->remove(ud.deleted_models[j]); delete ud.deleted_models[j]; } - for(unsigned int j = 0; j < ud.updated_models.size(); j++){ database->remove(ud.updated_models[j]); models_deleted_pub.publish(getModelMSG(ud.updated_models[j])); } - - - for(unsigned int j = 0; j < ud.updated_models.size(); j++){ - addToDB(database, ud.updated_models[j], true); - } - - for(unsigned int j = 0; j < ud.new_models.size(); j++){ - addToDB(database, ud.new_models[j], true); - } + for(unsigned int j = 0; j < ud.updated_models.size(); j++){ addToDB(database, ud.updated_models[j], true);} + for(unsigned int j = 0; j < ud.new_models.size(); j++){ addToDB(database, ud.new_models[j], true);} return true; } }else{ @@ -533,143 +389,6 @@ bool runSearch(ModelDatabase * database, reglib::Model * model, int number_of_se cv::waitKey( 0 ); } } - - // int maxval = 0; - // int maxind = 0; - - // int dilation_size = 0; - // cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, - // cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), - // cv::Point( dilation_size, dilation_size ) ); - // for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ - // //framekeys - - // cv_bridge::CvImagePtr ret_image_ptr; - // try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv_bridge::CvImagePtr ret_mask_ptr; - // try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv_bridge::CvImagePtr ret_depth_ptr; - // try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv::Mat rgbimage = ret_image_ptr->image; - // cv::Mat maskimage = ret_mask_ptr->image; - // cv::Mat depthimage = ret_depth_ptr->image; - - // cv::Mat erosion_dst; - // cv::erode( maskimage, erosion_dst, element ); - - // unsigned short * depthdata = (unsigned short * )depthimage.data; - // unsigned char * rgbdata = (unsigned char * )rgbimage.data; - // unsigned char * maskdata = (unsigned char * )erosion_dst.data; - // int count = 0; - // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ - // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ - // count++; - // } - // } - // if(count > maxval){ - // maxval = count; - // maxind = j; - // } - // } - - // //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ - // if(maxval > 100){//Atleast some pixels has to be in the mask... - // std::string key = sresult.retrieved_image_paths[i].strings[maxind]; - // printf("searchresult key:%s\n",key.c_str()); - // if(framekeys.count(key) == 0){ - - // int j = maxind; - - // cv_bridge::CvImagePtr ret_image_ptr; - // try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv_bridge::CvImagePtr ret_mask_ptr; - // try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv_bridge::CvImagePtr ret_depth_ptr; - // try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} - // catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - // cv::Mat rgbimage = ret_image_ptr->image; - // cv::Mat maskimage = ret_mask_ptr->image; - // cv::Mat depthimage = ret_depth_ptr->image; - // for (int ii = 0; ii < depthimage.rows; ++ii) { - // for (int jj = 0; jj < depthimage.cols; ++jj) { - // depthimage.at(ii, jj) *= 5; - // } - // } - - - // cv::Mat erosion_dst; - // cv::erode( maskimage, erosion_dst, element ); - - // // cv::Mat overlap = rgbimage.clone(); - // // unsigned short * depthdata = (unsigned short * )depthimage.data; - // // unsigned char * rgbdata = (unsigned char * )rgbimage.data; - // // unsigned char * maskdata = (unsigned char * )maskimage.data; - // // unsigned char * overlapdata = (unsigned char * )overlap.data; - // // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ - // // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ - // // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; - // // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; - // // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; - // // }else{ - // // overlapdata[3*pixel+0] = 0; - // // overlapdata[3*pixel+1] = 0; - // // overlapdata[3*pixel+2] = 0; - // // } - // // } - // // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); - // // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); - // // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); - // // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); - - // Eigen::Affine3d epose = Eigen::Affine3d::Identity(); - // reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); - // frame->keyval = key; - - // // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); - // // frame->show(true); - // //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); - // reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); - // bool res = searchmodel->testFrame(0); - - // reglib::Model * searchmodelHolder = new reglib::Model(); - // searchmodelHolder->submodels.push_back(searchmodel); - // searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); - // searchmodelHolder->last_changed = ++current_model_update; - // searchmodelHolder->recomputeModelPoints(); - - // //searchmodelHolder->showHistory(viewer); - - - // // if(cv::waitKey(0) != 'n'){ - // //Todo:: check new model is not flat or L shape - - // printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); - // //addToDB(modeldatabase, searchmodel,false,true); - // addToDB(modeldatabase, searchmodelHolder,false,true); - // show_sorted(); - // } - // } - -// printf("---------------ids in searchresult: %i----------------\n",result.vocabulary_ids.size()); -// for(unsigned int i = 0; i < result.vocabulary_ids.size(); i++){ -// int ind = result.vocabulary_ids[i]; -// printf("found object with ind: %i\n",ind); -// if(vocabulary_ids.count(ind) == 0){ printf("does not exist in db, continue\n"); continue; } -// reglib::Model * search_model = vocabulary_ids[ind]; -// if(search_model != model){printf("adding to search results\n"); ret.push_back(search_model);} -// if(ret.size() == number_of_matches){printf("found enough search results\n"); return ret;} -// } }else{ ROS_ERROR("retrieval_client service FAIL in %s :: %i !",__FILE__,__LINE__); } @@ -683,24 +402,21 @@ std::set searchset; void addNewModel(reglib::Model * model){ reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reg->visualizationLvl = show_reg_lvl; reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model, reg); mu->occlusion_penalty = occlusion_penalty; mu->massreg_timeout = massreg_timeout; mu->viewer = viewer; - mu->show_init_lvl = show_init_lvl;//init show mu->show_refine_lvl = show_refine_lvl;//refine show mu->show_scoring = show_scoring;//fuse scoring show - reg->visualizationLvl = show_reg_lvl; - mu->makeInitialSetup(); - delete mu; delete reg; - reglib::Model * newmodelHolder = new reglib::Model(); + model->parrent = newmodelHolder; newmodelHolder->submodels.push_back(model); newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); newmodelHolder->last_changed = ++current_model_update; @@ -724,7 +440,6 @@ void addNewModel(reglib::Model * model){ searchset.insert(current->id); printf("new search %i\n",current->id); - if(runSearch(modeldatabase, current)){ do_next = true; break; @@ -740,535 +455,23 @@ void addNewModel(reglib::Model * model){ dumpDatabase(savepath); } - -void somaCallback(const std_msgs::String & m){ - printf("somaCallback(%s)\n",m.data.c_str()); -// quasimodo_msgs::model mod = m; -// addNewModel(quasimodo_brain::getModelFromMSG(mod,false)); -} +void somaCallback(const std_msgs::String & m){printf("somaCallback(%s)\n",m.data.c_str());} void modelCallback(const quasimodo_msgs::model & m){ quasimodo_msgs::model mod = m; addNewModel(quasimodo_brain::getModelFromMSG(mod,false)); } -bool nextNew = true; -reglib::Model * newmodel = 0; - -//std::vector newmasks; -std::vector allmasks; - -int modaddcount = 0; -bool modelFromFrame(quasimodo_msgs::model_from_frame::Request & req, quasimodo_msgs::model_from_frame::Response & res){ - //printf("======================================\nmodelFromFrame\n======================================\n"); - uint64 frame_id = req.frame_id; - uint64 isnewmodel = req.isnewmodel; - - printf("modelFromFrame %i and %i\n",int(frame_id),int(isnewmodel)); - - cv_bridge::CvImagePtr mask_ptr; - try{ mask_ptr = cv_bridge::toCvCopy(req.mask, sensor_msgs::image_encodings::MONO8);} - catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());return false;} - - cv::Mat mask = mask_ptr->image; - allmasks.push_back(mask); - //std::cout << frames[frame_id]->pose << std::endl << std::endl; - - if(newmodel == 0){ - newmodel = new reglib::Model(frames[frame_id],mask); - //modeldatabase->add(newmodel); - }else{ - newmodel->frames.push_back(frames[frame_id]); - newmodel->relativeposes.push_back(newmodel->frames.front()->pose.inverse() * frames[frame_id]->pose); - newmodel->modelmasks.push_back(new reglib::ModelMask(mask)); - newmodel->recomputeModelPoints(); - } - newmodel->modelmasks.back()->sweepid = sweepid_counter; - - res.model_id = newmodel->id; - if(isnewmodel != 0){ - reglib::RGBDFrame * frontframe = newmodel->frames.front(); - int current_model_update_before = current_model_update; - newmodel->recomputeModelPoints(); - - - reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); - reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( newmodel, reg); - mu->occlusion_penalty = occlusion_penalty; - mu->massreg_timeout = massreg_timeout; - mu->viewer = viewer; - - mu->show_init_lvl = show_init_lvl;//init show - mu->show_refine_lvl = show_refine_lvl;//refine show - mu->show_scoring = show_scoring;//fuse scoring show - reg->visualizationLvl = show_reg_lvl; - - //newmodel->print(); - mu->makeInitialSetup(); - //newmodel->print(); - delete mu; - delete reg; - - reglib::Model * newmodelHolder = new reglib::Model(); - newmodelHolder->submodels.push_back(newmodel); - newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); - newmodelHolder->last_changed = ++current_model_update; - newmodelHolder->recomputeModelPoints(); - - modeldatabase->add(newmodelHolder); - addToDB(modeldatabase, newmodelHolder,false); - for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - publish_history(modeldatabase->models[i]->getHistory()); - } - /* - for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - publish_history(modeldatabase->models[i]->getHistory()); - }*/ - show_sorted(); - publishDatabasePCD(); - dumpDatabase(savepath); - - //exit(0); - - - /* -//SIMPLE TEST -reglib::Model * testmodel = new reglib::Model(); -for(int i = 0; i < newmodel->frames.size();i++){ - printf("%i / %i\n",i+1,newmodel->frames.size()); - reglib::Model * tmp = new reglib::Model(); - tmp->frames.push_back(newmodel->frames[i]); - tmp->modelmasks.push_back(newmodel->modelmasks[i]); - tmp->relativeposes.push_back(Eigen::Matrix4d::Identity()); - tmp->recomputeModelPoints(); - - std::vector cp; - std::vector cf; - std::vector cm; - for(unsigned int i = 0; i < tmp->relativeposes.size(); i++){ - cp.push_back(tmp->relativeposes[i]); - cf.push_back(tmp->frames[i]); - cm.push_back(tmp->modelmasks[i]); - } - mu->getGoodCompareFrames(cp,cf,cm); - tmp->rep_relativeposes = cp; - tmp->rep_frames = cf; - tmp->rep_modelmasks = cm; - -// pcl::PointCloud::Ptr cloud = tmp->getPCLnormalcloud(1,false); -// viewer->removeAllPointClouds(); -// viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); -// viewer->spin(); - - testmodel->submodels.push_back(tmp); - testmodel->submodels_relativeposes.push_back(newmodel->relativeposes[i]); -} - -testmodel->recomputeModelPoints(); -{ - pcl::PointCloud::Ptr cloud = testmodel->getPCLnormalcloud(1,false); - viewer->removeAllPointClouds(); - viewer->addPointCloud (cloud, pcl::visualization::PointCloudColorHandlerRGBField(cloud), "cloud"); - viewer->spin(); -} - -std::vector testmodels; -std::vector testrps; -mu->addModelsToVector(testmodels,testrps,testmodel,Eigen::Matrix4d::Identity()); - -printf("testmodels: %i\n",testmodels.size()); -std::vector > testocs = mu->computeOcclusionScore(testmodels,testrps); -std::vector > scores = mu->getScores(testocs); -std::vector partition = mu->getPartition(scores,2,5,2); - -for(unsigned int i = 0; i < scores.size(); i++){ - for(unsigned int j = 0; j < scores.size(); j++){ - if(scores[i][j] > 0){printf(" ");} - printf("%5.5f ",0.0001*scores[i][j]); - } - printf("\n"); -} - -printf("partition"); -for(unsigned int i = 0; i < partition.size(); i++){printf("%i ", partition[i]);} -printf("\n"); -*/ - - //exit(0); - /* -vector models; -vector rps; - -// models.push_back(model); -// models.push_back(model2); -// rps.push_back(Eigen::Matrix4d::Identity()); -// rps.push_back(pose); - -addModelsToVector(models,rps,model,Eigen::Matrix4d::Identity()); -unsigned int nr_models1 = models.size(); -addModelsToVector(models,rps,model2,pose); -vector > ocs = computeOcclusionScore(models,rps); -*/ - /* - - //newmodel->recomputeModelPoints(); - - - newmodel->last_changed = ++current_model_update; - newmodel->print(); -//exit(0); - addToDB(modeldatabase, newmodel,false); - //if(modaddcount % 1 == 0){show_sorted();} - //show_sorted(); - publishDatabasePCD(); - modaddcount++; - //dumpDatabase(savepath); -*/ - - for(unsigned int m = 0; run_search && m < modeldatabase->models.size(); m++){ - printf("looking at: %i\n",int(modeldatabase->models[m]->last_changed)); - reglib::Model * currentTest = modeldatabase->models[m]; - if(currentTest->last_changed > current_model_update_before){ - printf("changed: %i\n",int(m)); - - double start = getTime(); - - new_search_result = false; - models_new_pub.publish(getModelMSG(currentTest)); - - while(getTime()-start < search_timeout){ - ros::spinOnce(); - if(new_search_result){ - - for(unsigned int i = 0; i < sresult.retrieved_images.size(); i++){ - - int maxval = 0; - int maxind = 0; - - int dilation_size = 0; - cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, - cv::Size( 2*dilation_size + 1, 2*dilation_size+1 ), - cv::Point( dilation_size, dilation_size ) ); - for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ - //framekeys - - cv_bridge::CvImagePtr ret_image_ptr; - try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv_bridge::CvImagePtr ret_mask_ptr; - try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv_bridge::CvImagePtr ret_depth_ptr; - try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv::Mat rgbimage = ret_image_ptr->image; - cv::Mat maskimage = ret_mask_ptr->image; - cv::Mat depthimage = ret_depth_ptr->image; - - cv::Mat erosion_dst; - cv::erode( maskimage, erosion_dst, element ); - - unsigned short * depthdata = (unsigned short * )depthimage.data; - unsigned char * rgbdata = (unsigned char * )rgbimage.data; - unsigned char * maskdata = (unsigned char * )erosion_dst.data; - int count = 0; - for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ - if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ - count++; - } - } - if(count > maxval){ - maxval = count; - maxind = j; - } - } - - //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ - if(maxval > 100){//Atleast some pixels has to be in the mask... - std::string key = sresult.retrieved_image_paths[i].strings[maxind]; - printf("searchresult key:%s\n",key.c_str()); - if(framekeys.count(key) == 0){ - - int j = maxind; - - cv_bridge::CvImagePtr ret_image_ptr; - try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv_bridge::CvImagePtr ret_mask_ptr; - try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv_bridge::CvImagePtr ret_depth_ptr; - try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv::Mat rgbimage = ret_image_ptr->image; - cv::Mat maskimage = ret_mask_ptr->image; - cv::Mat depthimage = ret_depth_ptr->image; - for (int ii = 0; ii < depthimage.rows; ++ii) { - for (int jj = 0; jj < depthimage.cols; ++jj) { - depthimage.at(ii, jj) *= 5; - } - } - - - cv::Mat erosion_dst; - cv::erode( maskimage, erosion_dst, element ); - - // cv::Mat overlap = rgbimage.clone(); - // unsigned short * depthdata = (unsigned short * )depthimage.data; - // unsigned char * rgbdata = (unsigned char * )rgbimage.data; - // unsigned char * maskdata = (unsigned char * )maskimage.data; - // unsigned char * overlapdata = (unsigned char * )overlap.data; - // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ - // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ - // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; - // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; - // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; - // }else{ - // overlapdata[3*pixel+0] = 0; - // overlapdata[3*pixel+1] = 0; - // overlapdata[3*pixel+2] = 0; - // } - // } - // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); - // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); - // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); - // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); - - Eigen::Affine3d epose = Eigen::Affine3d::Identity(); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); - frame->keyval = key; - - // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); - // frame->show(true); - //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); - reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); - bool res = searchmodel->testFrame(0); - - reglib::Model * searchmodelHolder = new reglib::Model(); - searchmodelHolder->submodels.push_back(searchmodel); - searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); - searchmodelHolder->last_changed = ++current_model_update; - searchmodelHolder->recomputeModelPoints(); - - //searchmodelHolder->showHistory(viewer); - - - // if(cv::waitKey(0) != 'n'){ - //Todo:: check new model is not flat or L shape - - printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); - //addToDB(modeldatabase, searchmodel,false,true); - //addToDB(modeldatabase, searchmodelHolder,false,true); - show_sorted(); - } - } - - - /* - for(unsigned int j = 0; j < sresult.retrieved_images[i].images.size(); j++){ - //framekeys - - cv_bridge::CvImagePtr ret_image_ptr; - try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv_bridge::CvImagePtr ret_mask_ptr; - try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv_bridge::CvImagePtr ret_depth_ptr; - try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv::Mat rgbimage = ret_image_ptr->image; - cv::Mat maskimage = ret_mask_ptr->image; - cv::Mat depthimage = ret_depth_ptr->image; - - cv::Mat erosion_dst; - cv::erode( maskimage, erosion_dst, element ); - - unsigned short * depthdata = (unsigned short * )depthimage.data; - unsigned char * rgbdata = (unsigned char * )rgbimage.data; - unsigned char * maskdata = (unsigned char * )erosion_dst.data; - int count = 0; - for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ - if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ - count++; - } - } - if(count > maxval){ - maxval = count; - maxind = j; - } - } - - //for(unsigned int j = 0; j < 1 && j < sresult.retrieved_images[i].images.size(); j++){ - if(maxval > 100){//Atleast some pixels has to be in the mask... - std::string key = sresult.retrieved_image_paths[i].strings[maxind]; - printf("searchresult key:%s\n",key.c_str()); - if(framekeys.count(key) == 0){ - - int j = maxind; - - cv_bridge::CvImagePtr ret_image_ptr; - try {ret_image_ptr = cv_bridge::toCvCopy(sresult.retrieved_images[i].images[j], sensor_msgs::image_encodings::BGR8);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv_bridge::CvImagePtr ret_mask_ptr; - try {ret_mask_ptr = cv_bridge::toCvCopy(sresult.retrieved_masks[i].images[j], sensor_msgs::image_encodings::MONO8);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv_bridge::CvImagePtr ret_depth_ptr; - try {ret_depth_ptr = cv_bridge::toCvCopy(sresult.retrieved_depths[i].images[j], sensor_msgs::image_encodings::MONO16);} - catch (cv_bridge::Exception& e) {ROS_ERROR("cv_bridge exception: %s", e.what());exit(-1);} - - cv::Mat rgbimage = ret_image_ptr->image; - cv::Mat maskimage = ret_mask_ptr->image; - cv::Mat depthimage = ret_depth_ptr->image; - for (int ii = 0; ii < depthimage.rows; ++ii) { - for (int jj = 0; jj < depthimage.cols; ++jj) { - depthimage.at(ii, jj) *= 5; - } - } - - - cv::Mat erosion_dst; - cv::erode( maskimage, erosion_dst, element ); - - // cv::Mat overlap = rgbimage.clone(); - // unsigned short * depthdata = (unsigned short * )depthimage.data; - // unsigned char * rgbdata = (unsigned char * )rgbimage.data; - // unsigned char * maskdata = (unsigned char * )maskimage.data; - // unsigned char * overlapdata = (unsigned char * )overlap.data; - // for(int pixel = 0; pixel < depthimage.rows*depthimage.cols;pixel++){ - // if(depthdata[pixel] > 0 && maskdata[pixel] > 0){ - // overlapdata[3*pixel+0] = rgbdata[3*pixel+0]; - // overlapdata[3*pixel+1] = rgbdata[3*pixel+1]; - // overlapdata[3*pixel+2] = rgbdata[3*pixel+2]; - // }else{ - // overlapdata[3*pixel+0] = 0; - // overlapdata[3*pixel+1] = 0; - // overlapdata[3*pixel+2] = 0; - // } - // } - // cv::namedWindow( "rgbimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "rgbimage", rgbimage ); - // cv::namedWindow( "maskimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "maskimage", maskimage ); - // cv::namedWindow( "depthimage", cv::WINDOW_AUTOSIZE ); cv::imshow( "depthimage", 100*depthimage ); - // cv::namedWindow( "overlap", cv::WINDOW_AUTOSIZE ); cv::imshow( "overlap", overlap ); - - Eigen::Affine3d epose = Eigen::Affine3d::Identity(); - reglib::RGBDFrame * frame = new reglib::RGBDFrame(cameras[0],rgbimage, depthimage, 0, epose.matrix()); - frame->keyval = key; - frame->show(true); - //reglib::Model * searchmodel = new reglib::Model(frame,maskimage); - reglib::Model * searchmodel = new reglib::Model(frame,erosion_dst); - bool res = searchmodel->testFrame(0); - - reglib::Model * searchmodelHolder = new reglib::Model(); - searchmodelHolder->submodels.push_back(searchmodel); - searchmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); - searchmodelHolder->last_changed = ++current_model_update; - searchmodelHolder->recomputeModelPoints(); - - - // if(cv::waitKey(0) != 'n'){ - //Todo:: check new model is not flat or L shape - - printf("--- trying to add serach results, if more then one addToDB: results added-----\n"); - //addToDB(modeldatabase, searchmodel,false,true); - addToDB(modeldatabase, searchmodelHolder,false,true); - show_sorted(); - // } - } - - } - */ - } - - break; - }else{ - printf("searching... timeout in %3.3f\n", +start +search_timeout - getTime()); - usleep(100000); - } - } - } - //exit(0); - } - - for(unsigned int i = 0; i < modeldatabase->models.size(); i++){ - for(unsigned int j = 0; j < modeldatabase->models[i]->submodels.size(); j++){ - if( modeldatabase->models[i]->submodels[j]->frames.front() == frontframe){ - Eigen::Matrix4d pose = frontframe->pose * modeldatabase->models[i]->submodels_relativeposes[j].inverse(); - - pcl::PointCloud::Ptr cloud = modeldatabase->models[i]->submodels[j]->getPCLcloud(1,true); - pcl::PointCloud::Ptr transformed_cloud (new pcl::PointCloud ()); - pcl::transformPointCloud (*cloud, *transformed_cloud, pose); - - - sensor_msgs::PointCloud2 input; - pcl::toROSMsg (*transformed_cloud,input);//, *transformed_cloud); - input.header.frame_id = "/map"; - model_last_pub.publish(input); - } - } - } - - newmodel = 0; - sweepid_counter++; - } - return true; -} - -//bool fuseModels(quasimodo_msgs::fuse_models::Request & req, quasimodo_msgs::fuse_models::Response & res){} - -int getdir (std::string dir, std::vector & files){ - DIR *dp; - struct dirent *dirp; - if((dp = opendir(dir.c_str())) == NULL) { - cout << "Error(" << errno << ") opening " << dir << endl; - return errno; - } - - while ((dirp = readdir(dp)) != NULL) { - files.push_back(std::string(dirp->d_name)); - } - closedir(dp); - return 0; -} - void clearMem(){ - - for(auto iterator = cameras.begin(); iterator != cameras.end(); iterator++) { - delete iterator->second; - } - - for(auto iterator = frames.begin(); iterator != frames.end(); iterator++) { - delete iterator->second; - } - - for(auto iterator = models.begin(); iterator != models.end(); iterator++) { + for(auto iterator = cameras.begin(); iterator != cameras.end(); iterator++) {delete iterator->second;} + for(auto iterator = frames.begin(); iterator != frames.end(); iterator++) {delete iterator->second;} + for(auto iterator = models.begin(); iterator != models.end(); iterator++) { reglib::Model * model = iterator->second; - for(unsigned int i = 0; i < model->frames.size(); i++){ - delete model->frames[i]; - } - - for(unsigned int i = 0; i < model->modelmasks.size(); i++){ - delete model->modelmasks[i]; - } + for(unsigned int i = 0; i < model->frames.size() ; i++){delete model->frames[i];} + for(unsigned int i = 0; i < model->modelmasks.size(); i++){delete model->modelmasks[i];} delete model; } - - for(auto iterator = updaters.begin(); iterator != updaters.end(); iterator++) { - delete iterator->second; - } - - + for(auto iterator = updaters.begin(); iterator != updaters.end(); iterator++) {delete iterator->second;} if(registration != 0){delete registration;} if(modeldatabase != 0){delete modeldatabase;} } @@ -1276,34 +479,20 @@ void clearMem(){ int main(int argc, char **argv){ cameras[0] = new reglib::Camera(); registration = new reglib::RegistrationRandom(); - modeldatabase = 0;//new ModelDatabaseRGBHistogram(5); - //new ModelDatabaseRetrieval(n); + modeldatabase = 0; ros::init(argc, argv, "quasimodo_model_server"); ros::NodeHandle n; + nh = &n; models_new_pub = n.advertise("/models/new", 1000); models_updated_pub = n.advertise("/models/updated", 1000); models_deleted_pub = n.advertise("/models/deleted", 1000); - ros::ServiceServer service1 = n.advertiseService("model_from_frame", modelFromFrame); - ROS_INFO("Ready to add use model_from_frame."); - - ros::ServiceServer service2 = n.advertiseService("index_frame", indexFrame); - ROS_INFO("Ready to add use index_frame."); - - //ros::ServiceServer service3 = n.advertiseService("fuse_models", fuseModels); - //ROS_INFO("Ready to add use fuse_models."); - - ros::ServiceServer service4 = n.advertiseService("get_model", getModel); - ROS_INFO("Ready to add use get_model."); - -//<<<<<<< HEAD -// soma2add = n.serviceClient("soma2/insert_objs"); -// ROS_INFO("Ready to add use soma2add."); -//======= -// soma_add = n.serviceClient("soma/insert_objs"); -// ROS_INFO("Ready to add use soma_add."); -//>>>>>>> integrate_object_search + //ros::ServiceServer service1 = n.advertiseService("model_from_frame", modelFromFrame); + ros::ServiceServer service2 = n.advertiseService("index_frame", indexFrame); + ros::ServiceServer service4 = n.advertiseService("get_model", getModel); + ros::ServiceServer service5 = n.advertiseService("quasimodo/recognize", recognizeService); + ROS_INFO("Ready to add use services."); ros::Subscriber sub = n.subscribe("/retrieval_result", 1, retrievalCallback); ROS_INFO("Ready to add recieve search results."); @@ -1331,7 +520,7 @@ int main(int argc, char **argv){ if( std::string(argv[i]).compare("-c") == 0){ printf("camera input state\n"); inputstate = 1;} else if(std::string(argv[i]).compare("-l") == 0){ printf("reading all models at %s (the path defined from -p)\n",savepath.c_str()); std::vector folderdata; - int r = getdir(savepath+"/",folderdata); + int r = quasimodo_brain::getdir(savepath+"/",folderdata); for(unsigned int fnr = 0; fnr < folderdata.size(); fnr++){ printf("%s\n",folderdata[fnr].c_str()); } @@ -1356,12 +545,11 @@ int main(int argc, char **argv){ else if(inputstate == 1){ reglib::Camera * cam = reglib::Camera::load(std::string(argv[i])); delete cameras[0]; - cameras[0] = cam; + cameras[0] = cam; }else if(inputstate == 2){ reglib::Model * model = reglib::Model::load(cameras[0],std::string(argv[i])); sweepid_counter = std::max(int(model->modelmasks[0]->sweepid + 1), sweepid_counter); modeldatabase->add(model); - //addToDB(modeldatabase, model,false); model->last_changed = ++current_model_update; show_sorted(); }else if(inputstate == 3){ @@ -1372,9 +560,7 @@ int main(int argc, char **argv){ massreg_timeout = atof(argv[i]); printf("massreg_timeout set to %f\n",massreg_timeout); }else if(inputstate == 6){ search_timeout = atof(argv[i]); printf("search_timeout set to %f\n",search_timeout); - if(search_timeout == 0){ - run_search = false; - } + if(search_timeout == 0){run_search = false;} }else if(inputstate == 8){ show_init_lvl = atoi(argv[i]); }else if(inputstate == 9){ @@ -1404,36 +590,57 @@ int main(int argc, char **argv){ } } + if(visualization){ + viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("Modelserver Viewer")); + viewer->addCoordinateSystem(0.1); + viewer->setBackgroundColor(1.0,0.0,1.0); + } + if(modeldatabase == 0){modeldatabase = new ModelDatabaseRetrieval(n);} for(unsigned int i = 0; i < modelpcds.size(); i++){ std::vector mods = quasimodo_brain::loadModelsPCDs(modelpcds[i]); for(unsigned int j = 0; j < mods.size(); j++){ - modeldatabase->add(mods[j]); - } - } + //modeldatabase->add(mods[j]); + //addNewModel(mods[j]); + reglib::Model * model = mods[j]; + + reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); + reg->visualizationLvl = show_reg_lvl; + reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model, reg); + mu->occlusion_penalty = occlusion_penalty; + mu->massreg_timeout = massreg_timeout; + mu->viewer = viewer; + mu->show_init_lvl = show_init_lvl;//init show + mu->show_refine_lvl = show_refine_lvl;//refine show + mu->show_scoring = show_scoring;//fuse scoring show + mu->makeInitialSetup(); - //if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/model/out/topic", 100, modelCallback));} - if(input_model_subs.size() == 0){input_model_subs.push_back(n.subscribe("/quasimodo/segmentation/out/model", 100, modelCallback));} + delete mu; + delete reg; + reglib::Model * newmodelHolder = new reglib::Model(); + model->parrent = newmodelHolder; + newmodelHolder->submodels.push_back(model); + newmodelHolder->submodels_relativeposes.push_back(Eigen::Matrix4d::Identity()); + newmodelHolder->last_changed = ++current_model_update; + if(show_modelbuild){ + newmodelHolder->recomputeModelPoints(Eigen::Matrix4d::Identity(),viewer); + }else{ + newmodelHolder->recomputeModelPoints(); + } + modeldatabase->add(newmodelHolder); + } + //show_sorted(); + } - if(soma_input_model_subs.size() == 0){soma_input_model_subs.push_back(n.subscribe("/quasimodo/segmentation/out/soma_segment_id", 10000, somaCallback));} + if(input_model_subs.size() == 0){input_model_subs.push_back( n.subscribe("/quasimodo/segmentation/out/model", 100, modelCallback));} + if(soma_input_model_subs.size() == 0){soma_input_model_subs.push_back( n.subscribe("/quasimodo/segmentation/out/soma_segment_id", 10000, somaCallback));} - if(visualization){ - viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("Modelserver Viewer")); - viewer->addCoordinateSystem(0.1); - viewer->setBackgroundColor(1.0,0.0,1.0); - } - ros::Duration(1.0).sleep(); - chatter_pub = n.advertise("modelserver", 1000); - std_msgs::String msg; - msg.data = "starting"; - chatter_pub.publish(msg); - ros::spinOnce(); ros::spin(); return 0; } diff --git a/quasimodo/quasimodo_models/include/model/Model.h b/quasimodo/quasimodo_models/include/model/Model.h index 3a7543c0..b795085b 100644 --- a/quasimodo/quasimodo_models/include/model/Model.h +++ b/quasimodo/quasimodo_models/include/model/Model.h @@ -54,6 +54,7 @@ using namespace Eigen; std::vector submodels; std::vector submodels_relativeposes; std::vector > submodels_scores; + Model * parrent; Model(); Model(RGBDFrame * frame_, cv::Mat mask, Eigen::Matrix4d pose = Eigen::Matrix4d::Identity()); diff --git a/quasimodo/quasimodo_models/include/registration/.MassRegistrationPPR2.h.swp b/quasimodo/quasimodo_models/include/registration/.MassRegistrationPPR2.h.swp new file mode 100644 index 0000000000000000000000000000000000000000..fee1de3600b4e4e22312b2c2a393d9bb78e5e69e GIT binary patch literal 16384 zcmeI3UyL199mmIl%2g^xB>14oq5IO3+nr_a7V&nySX>HCT~n6+SqaH>cIM2z!_J)< z=A7B>y0irUJeXK5YJ4Foi2;f7=9`ID4H9|qK^_Rah=0%*3sgm(XyW&r`E%#r-Mth@ zOnN5y^zO|0oqyl+n=|KkX1X$S{Lmu#^z@vD(?eE*YoYmZo(_CPQ&eJT>R zJ`%e@Fx$ zuK(DvD@g&>SEYbbKq;UUPzopolmbctrN9-h01tO)4?~3=sS=y%@B1phSEc8B)8B_G z>FKHZQ3@yplmbctrGQdEDWDWk3Md7X0!jg;fKuT9Pyy4{wA;|G&xs8Hy#LSQ|G!+P zX}<-(0Y3(hf^*k z>)>l30uO?d;1HMtH-L|W9pK_lO?w@@2+o72!8yRdF>ovR1h{mqro9E82Y&>=2Is-! z;78yq;1O^;xb$I7`vvg8r@(6;(zI8>1@KRB9()%#U=Hj6ziesRPr##K6Hss%Oo3Of zL4Uz{@GJ0B@Kta>m;*b(pFfB)z&Q|rS#aS4n)VWS7W^DM4!!{_po3|!7u*Q``+iM( z2Al==fhq9j)u#Y{!cDK7m zEgnSk{!kL}mq zrSh!63!-ij^fIF^#Z?N=|*JOCO5}fvUN&B*G4i*TEO=!rf<;`)a?)W!PvLN zw=A>b+ZA#%v1!N$)b3NmL5iq3k+PH&nSP(1Fw7_#W3U`tqR8A>zzB2}QrGK2uwrw~j=feL zM{+cxv}0E+IWDq}T{F(dp;WA>*!3OZ#$p&{({#P0fb6ws4UR%@#PF{KvG7)rB92nF z@%1%(B}vqU!{jam#o#06#wdfXORJKJUua3IwGsp#i5WF=8+dKy254-k7*#nw%nG7p zjJ9_5nuCvV>t(;@%nkm1Pu(hW^hDf>yd|xRCq;LEhfXfDk9Y0MaOmyacmn5@9#M7K zq5!1edD1wWRLl2g%`BhGx3L>}W2x56ed|t}yCjPv$F!)NMjP@beEDuHr5vS< zFN!%+cCANHhV;pXDKy5D?$$Yji`lT%f}tFGn=&hMLp1;a4rRQ9f#7KFTd2~S$rjf3nyEzqs z@gQ6?J*kTvg~U(XFVkeQSf^s4Ds$vBu|rKBr%(C(B9^d5Vk|ll+ zj&6jMaQq`Lg$NT|tE;s(wU*`Kb8gb~O*iS1#ITl%N&d8`>`Xq>?cTF_T1@??LCeOl z+lDY`*>DY_ay#)Kk)fTf!EIyDrYJiVPOcGU23Uzwv&`kL zCH=~)Kna@~Vj;>xJj)0^X!qO|1as*Z?qpuLXYC)@GRow-+^bqx4>C&2RH!U zKwB?@*TJjc0ub$g1*k(QpcGIFC< z?cUNV3>am5QjEe^9~CI6S+W7OE%K2mC9AI5zX4k zO5Tka$GEpz#TFf=8%N|G`FqebC#^B7l2ID&X>P*-_l)o zd{Mq&=)s0Zi1SU)Gb~{!TsZAYQ@O~wa8PU{-P|K`Lx~|EDBc$`KEt|vmB3;+#2-nC zVyS*GW(XPKFtNv(xF?Qgy8_!dc0Z;py|<*urftg}k2*C$CuR3Wz01lN^(g63n-sc` z^q`bUrpnsoUs<4?ItYtbhVnn=B8cKqOp!I)WzAYHv+XWr9#vp@)Qn{5xQ2l^Qw?!M z6ti2}H>TvFD}r`JP-w->^vu4raceF^M2wjXmmP%0qIKe^y<-})zb9V^q!DiV8m5{x z-H^zPFwgGS^#ufO*n9|l5;(+$s}hs2kXo+eS|l|kFlBAxIs~tI@{2Ab7$021AD9GV z&n7FB?5nl7Qf;zlF=55hc+#pnO>!0ezjQOC%@W(Eg?D>$%91;&W7I3y7PrEy*2Fjc z!0}8UuNMz5h<7#zY*TDW&X=aQ&ay4vHx{gi6gzg7@r7CAUUwLJG viewer){ @@ -469,6 +471,7 @@ void Model::merge(Model * model, Eigen::Matrix4d p){ for(unsigned int i = 0; i < model->submodels.size(); i++){ submodels_relativeposes.push_back(p * model->submodels_relativeposes[i]); submodels.push_back(model->submodels[i]); + model->submodels[i]->parrent = this; } recomputeModelPoints(); diff --git a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp index 840f7cf3..d4e24940 100644 --- a/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp +++ b/quasimodo/quasimodo_models/src/modelupdater/ModelUpdaterBasicFuse.cpp @@ -226,13 +226,7 @@ UpdatedModels ModelUpdaterBasicFuse::fuseData(FusionResults * f, Model * model1, for(unsigned int i = 1; i < count.size(); i++){minpart = std::min(minpart,count[i]);} if(count.size() == 1){ - printf("points before: %i\n",model1->points.size()); model1->merge(model2,pose); - - model1->recomputeModelPoints(); - printf("points after: %i\n",model1->points.size()); - //model1->scores = scores; - //model1->total_scores = sumscore; retval.updated_models.push_back(model1); retval.deleted_models.push_back(model2); }else if(improvement > 1){//Cannot fully fuse... separating... diff --git a/quasimodo/quasimodo_msgs/CMakeLists.txt b/quasimodo/quasimodo_msgs/CMakeLists.txt index 3f2a729b..eef27298 100644 --- a/quasimodo/quasimodo_msgs/CMakeLists.txt +++ b/quasimodo/quasimodo_msgs/CMakeLists.txt @@ -74,6 +74,7 @@ add_service_files( mask_pointclouds.srv insert_model.srv model_to_retrieval_query.srv + recognize.srv ) ## Generate actions in the 'action' folder diff --git a/quasimodo/quasimodo_msgs/srv/recognize.srv b/quasimodo/quasimodo_msgs/srv/recognize.srv new file mode 100644 index 00000000..329bb9da --- /dev/null +++ b/quasimodo/quasimodo_msgs/srv/recognize.srv @@ -0,0 +1,3 @@ +string id +--- +string id From f8c452e6683106a481d99db2373cdd184b8f92e8 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Tue, 29 Nov 2016 09:50:20 +0100 Subject: [PATCH 253/255] ... --- quasimodo/quasimodo_brain/CMakeLists.txt | 11 +++++ .../src/ModelDatabase/ModelDatabase.cpp | 4 +- .../src/ModelDatabase/ModelDatabase.h | 3 ++ .../src/ModelDatabase/ModelDatabaseBasic.cpp | 4 +- .../ModelDatabaseRGBHistogram.cpp | 3 ++ .../ModelDatabase/ModelDatabaseRetrieval.cpp | 12 ++--- .../src/ModelStorage/ModelStorage.cpp | 14 ++++++ .../src/ModelStorage/ModelStorage.h | 28 +++++++++++ .../src/ModelStorage/ModelStorageFile.cpp | 48 +++++++++++++++++++ .../src/ModelStorage/ModelStorageFile.h | 18 +++++++ quasimodo/quasimodo_brain/src/Util/Util.cpp | 9 ++++ quasimodo/quasimodo_brain/src/Util/Util.h | 2 + .../metaroom_additional_view_processing.cpp | 7 ++- quasimodo/quasimodo_brain/src/modelserver.cpp | 44 ++++++++++++++++- .../quasimodo_models/src/model/Model.cpp | 9 ++-- 15 files changed, 201 insertions(+), 15 deletions(-) create mode 100644 quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.cpp create mode 100644 quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.h create mode 100644 quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.cpp create mode 100644 quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.h diff --git a/quasimodo/quasimodo_brain/CMakeLists.txt b/quasimodo/quasimodo_brain/CMakeLists.txt index 9451bf85..c4096314 100644 --- a/quasimodo/quasimodo_brain/CMakeLists.txt +++ b/quasimodo/quasimodo_brain/CMakeLists.txt @@ -75,8 +75,19 @@ target_link_libraries(quasimodo_brain_util ${PCL_LIBRARIES} ${catkin_LIBRARIES}) +add_library(quasimodo_ModelStorage src/ModelStorage/ModelStorage.cpp src/ModelStorage/ModelStorageFile.cpp) +target_link_libraries(quasimodo_ModelStorage + quasimodo_brain_util + #${retrieval_libraries} + #${benchmark_libraries} + ${OpenCV_LIBS} + ${QT_QTMAIN_LIBRARY} ${QT_LIBRARIES} + ${PCL_LIBRARIES} + ${catkin_LIBRARIES}) + add_library(quasimodo_ModelDatabase src/ModelDatabase/ModelDatabase.cpp src/ModelDatabase/ModelDatabaseBasic.cpp src/ModelDatabase/ModelDatabaseRGBHistogram.cpp src/ModelDatabase/ModelDatabaseRetrieval.cpp ) target_link_libraries(quasimodo_ModelDatabase + quasimodo_ModelStorage quasimodo_brain_util #${retrieval_libraries} #${benchmark_libraries} ${OpenCV_LIBS} diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.cpp index 8e91b514..5260f8c9 100644 --- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.cpp +++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.cpp @@ -1,16 +1,18 @@ #include "ModelDatabase.h" -ModelDatabase::ModelDatabase(){} +ModelDatabase::ModelDatabase(){storage = new ModelStorageFile();} ModelDatabase::~ModelDatabase(){} //Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching bool ModelDatabase::add(reglib::Model * model){ + storage->add(model); return true; } // return true if successfull // return false if fail bool ModelDatabase::remove(reglib::Model * model){ + storage->remove(model); return false; } diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h index 60d7338a..e55061ae 100644 --- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h +++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabase.h @@ -10,9 +10,12 @@ #include "model/Model.h" #include "../Util/Util.h" +#include "../ModelStorage/ModelStorage.h" + class ModelDatabase{ public: std::vector models; + ModelStorage * storage; //Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching virtual bool add(reglib::Model * model); diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.cpp index b948eb18..f39997a5 100644 --- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.cpp +++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseBasic.cpp @@ -1,16 +1,18 @@ #include "ModelDatabaseBasic.h" -ModelDatabaseBasic::ModelDatabaseBasic(){} +ModelDatabaseBasic::ModelDatabaseBasic(){storage = new ModelStorageFile();} ModelDatabaseBasic::~ModelDatabaseBasic(){} bool ModelDatabaseBasic::add(reglib::Model * model){ + storage->add(model); models.push_back(model); return true; //printf("number of models: %i\n",models.size()); } bool ModelDatabaseBasic::remove(reglib::Model * model){ + storage->remove(model); for(unsigned int i = 0; i < models.size(); i++){ if(models[i] == model){ models[i] = models.back(); diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.cpp index 7ee204e9..1cce7d32 100644 --- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.cpp +++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRGBHistogram.cpp @@ -2,6 +2,7 @@ ModelDatabaseRGBHistogram::ModelDatabaseRGBHistogram(int res_){ res = res_; + storage = new ModelStorageFile(); printf("made a ModelDatabaseRGBHistogram(%i)\n",res); } ModelDatabaseRGBHistogram::~ModelDatabaseRGBHistogram(){} @@ -27,6 +28,7 @@ std::vector< double > getDescriptor(int res, reglib::Model * model){ } bool ModelDatabaseRGBHistogram::add(reglib::Model * model){ + storage->add(model); //std::vector< std::vector< double > > descriptors; std::vector< double > descriptor = getDescriptor(res,model); descriptors.push_back(descriptor); @@ -36,6 +38,7 @@ bool ModelDatabaseRGBHistogram::add(reglib::Model * model){ } bool ModelDatabaseRGBHistogram::remove(reglib::Model * model){ + storage->remove(model); for(unsigned int i = 0; i < models.size(); i++){ if(models[i] == model){ models[i] = models.back(); diff --git a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp index a3020832..a12d9f86 100644 --- a/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp +++ b/quasimodo/quasimodo_brain/src/ModelDatabase/ModelDatabaseRetrieval.cpp @@ -9,7 +9,7 @@ ModelDatabaseRetrieval::ModelDatabaseRetrieval(ros::NodeHandle & n, std::string retrieval_client = n.serviceClient (retrieval_name); conversion_client = n.serviceClient (conversion_name); insert_client = n.serviceClient (insert_name); - + storage = new ModelStorageFile(); //mongo::DBClientConnection c; //c.connect("localhost"); //c.dropCollection("databaseName.collectionName"); @@ -18,6 +18,7 @@ ModelDatabaseRetrieval::ModelDatabaseRetrieval(ros::NodeHandle & n, std::string ModelDatabaseRetrieval::~ModelDatabaseRetrieval(){} bool ModelDatabaseRetrieval::add(reglib::Model * model){ + storage->add(model); quasimodo_msgs::insert_model im; im.request.model = quasimodo_brain::getModelMSG(model,true); im.request.action = im.request.INSERT; @@ -35,6 +36,7 @@ bool ModelDatabaseRetrieval::add(reglib::Model * model){ } bool ModelDatabaseRetrieval::remove(reglib::Model * model){ + storage->remove(model); for(unsigned int i = 0; i < models.size(); i++){ if(models[i] == model){ //Remove stuff @@ -63,15 +65,13 @@ bool ModelDatabaseRetrieval::remove(reglib::Model * model){ } std::vector ModelDatabaseRetrieval::search(reglib::Model * model, int number_of_matches){ - +printf("start: %s\n",__PRETTY_FUNCTION__); printf("---------------ids in database: %i----------------\n",v_ids.size()); for(unsigned int i = 0; i < v_ids.size(); i++){ printf("v_ids[%i] = %i\n",i,v_ids[i]); } - printf("----------------------------------------------\n"); std::vector ret; - quasimodo_msgs::model_to_retrieval_query m2r; m2r.request.model = quasimodo_brain::getModelMSG(model,true); if (conversion_client.call(m2r)){ @@ -79,17 +79,14 @@ std::vector ModelDatabaseRetrieval::search(reglib::Model * mode qc.request.query = m2r.response.query; qc.request.query.query_kind = qc.request.query.MONGODB_QUERY; qc.request.query.number_query = number_of_matches+10; - if (retrieval_client.call(qc)){ quasimodo_msgs::retrieval_result result = qc.response.result; - printf("---------------ids in searchresult: %i----------------\n",result.vocabulary_ids.size()); for(unsigned int i = 0; i < result.vocabulary_ids.size(); i++){ int ind = std::stoi(result.vocabulary_ids[i]); printf("found object with ind: %i\n",ind); } printf("----------------------------------------------\n"); - for(unsigned int i = 0; i < result.vocabulary_ids.size(); i++){ int ind = std::stoi(result.vocabulary_ids[i]); printf("found object with ind: %i\n",ind); @@ -104,5 +101,6 @@ std::vector ModelDatabaseRetrieval::search(reglib::Model * mode }else{ ROS_ERROR("model_to_retrieval_query service FAIL!"); } +printf("stop: %s\n",__PRETTY_FUNCTION__); return ret; } diff --git a/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.cpp b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.cpp new file mode 100644 index 00000000..cccdcfe3 --- /dev/null +++ b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.cpp @@ -0,0 +1,14 @@ +#include "ModelStorage.h" + +ModelStorage::ModelStorage(){} +ModelStorage::~ModelStorage(){} + +//Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching +bool ModelStorage::add(reglib::Model * model, std::string parrentpath){ + return true; +} + +bool ModelStorage::remove(reglib::Model * model){ + return false; +} + diff --git a/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.h b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.h new file mode 100644 index 00000000..1669f93a --- /dev/null +++ b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorage.h @@ -0,0 +1,28 @@ +#ifndef ModelStorage_H +#define ModelStorage_H + +#include + +// PCL specific includes +#include +#include + +#include "model/Model.h" +#include "../Util/Util.h" + +class ModelStorage{ + public: + + //Add pointcloud to database, return index number in database, weight is the bias of the system to perfer this object when searching + virtual bool add(reglib::Model * model, std::string parrentpath = ""); + + // return true if successfull + // return false if fail + virtual bool remove(reglib::Model * model); + + ModelStorage(); + ~ModelStorage(); +}; + +#include "ModelStorageFile.h" +#endif // ModelStorage_H diff --git a/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.cpp b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.cpp new file mode 100644 index 00000000..9bfe3597 --- /dev/null +++ b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.cpp @@ -0,0 +1,48 @@ +#include "ModelStorageFile.h" + +ModelStorageFile::ModelStorageFile(std::string filepath_){ + filepath = filepath_; + if(!quasimodo_brain::fileExists(filepath+"/database_tmp")){ + char command [1024]; + sprintf(command,"mkdir %s",(filepath+"/database_tmp").c_str()); + int r = system(command); + if(!quasimodo_brain::fileExists(filepath+"/database_tmp/frames")){ + sprintf(command,"mkdir %s",(filepath+"/database_tmp/frames").c_str()); + r = system(command); + } + } +} +ModelStorageFile::~ModelStorageFile(){} + + +bool ModelStorageFile::add(reglib::Model * model, std::string parrentpath){ +// printf("%s\n",__PRETTY_FUNCTION__); + + +// for(unsigned int i = 0; i < model->frames.size(); i++){//Add all frames to the frame database +// std::string keyval = model->frames[i]->keyval; +// printf("keyval: %s\n",keyval.c_str()); +// } + + +// for(unsigned int i = 0; i < model->submodels.size(); i++){//Add all frames to the frame database +// add(model->submodels[i],parrentpath+"/sub"); +// } + // models.push_back(model); + // return true; + // //printf("number of models: %i\n",models.size()); + //exit(0); +} + +bool ModelStorageFile::remove(reglib::Model * model){ + //printf("%s\n",__PRETTY_FUNCTION__); + // for(unsigned int i = 0; i < models.size(); i++){ + // if(models[i] == model){ + // models[i] = models.back(); + // models.pop_back(); + // return true; + // } + // } + // return false; + //exit(0); +} diff --git a/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.h b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.h new file mode 100644 index 00000000..78d83fce --- /dev/null +++ b/quasimodo/quasimodo_brain/src/ModelStorage/ModelStorageFile.h @@ -0,0 +1,18 @@ +#ifndef ModelStorageFile_H +#define ModelStorageFile_H + +#include "ModelStorage.h" + + +class ModelStorageFile: public ModelStorage{ + public: + std::string filepath; + virtual bool add(reglib::Model * model, std::string parrentpath = ""); + virtual bool remove(reglib::Model * model); + ModelStorageFile(std::string filepath_ = "./"); + ~ModelStorageFile(); +}; + + + +#endif // ModelStorageFile_H diff --git a/quasimodo/quasimodo_brain/src/Util/Util.cpp b/quasimodo/quasimodo_brain/src/Util/Util.cpp index 040907cd..f809390a 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.cpp +++ b/quasimodo/quasimodo_brain/src/Util/Util.cpp @@ -2,6 +2,15 @@ namespace quasimodo_brain { +bool fileExists(std::string path){ + QFile file(path.c_str()); + if (file.exists()){ + return true; + }else{ + return false; + } +} + int getdir (std::string dir, std::vector & files){ DIR *dp; struct dirent *dirp; diff --git a/quasimodo/quasimodo_brain/src/Util/Util.h b/quasimodo/quasimodo_brain/src/Util/Util.h index ae708fd7..21f11705 100644 --- a/quasimodo/quasimodo_brain/src/Util/Util.h +++ b/quasimodo/quasimodo_brain/src/Util/Util.h @@ -44,6 +44,8 @@ #include namespace quasimodo_brain { + +bool fileExists(std::string path); int getdir (std::string dir, std::vector & files); std::string getPoseString(Eigen::Matrix4d pose); diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index efe224a1..944a494e 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -93,6 +93,7 @@ typedef semantic_map_load_utilties::DynamicObjectData ObjectData; using namespace std; using namespace semantic_map_load_utilties; +int minClusterSize = 2000; std::string overall_folder = ""; boost::shared_ptr viewer; int visualization_lvl = 0; @@ -993,7 +994,7 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr printf("peopleoverlaps: %i\n",peopleoverlaps); if(masks.size() > 0){ - if(peopleoverlaps == 0){ + if(peopleoverlaps == 0 && cloud_cluster->points.size() >= minClusterSize){ if(store_old_xml){ std::stringstream ss; @@ -2255,7 +2256,6 @@ int main(int argc, char** argv){ std::vector raresfiles; bool resegment = false; - int inputstate = 0; for(int i = 1; i < argc;i++){ printf("input: %s\n",argv[i]); @@ -2284,6 +2284,7 @@ int main(int argc, char** argv){ else if(std::string(argv[i]).compare("-v_reg") == 0){ visualization_lvl_regref = 1;inputstate = 18;} else if(std::string(argv[i]).compare("-saveVisuals") == 0){ inputstate = 19;} else if(std::string(argv[i]).compare("-testpaths") == 0 || std::string(argv[i]).compare("-testpath") == 0 || std::string(argv[i]).compare("-testPaths") == 0 || std::string(argv[i]).compare("-testPath") == 0){ inputstate = 20;} + else if(std::string(argv[i]).compare("-minClusterSize") == 0){ inputstate = 21;} else if(inputstate == 0){ segsubs.push_back(n.subscribe(std::string(argv[i]), 1000, chatterCallback)); }else if(inputstate == 1){ @@ -2327,6 +2328,8 @@ int main(int argc, char** argv){ saveVisuals = std::string(argv[i]); }else if(inputstate == 20){ testpaths.push_back(std::string(argv[i])); + }else if(inputstate == 21){ + minClusterSize = atoi(argv[i]); } } diff --git a/quasimodo/quasimodo_brain/src/modelserver.cpp b/quasimodo/quasimodo_brain/src/modelserver.cpp index 2980fba4..4b627df3 100644 --- a/quasimodo/quasimodo_brain/src/modelserver.cpp +++ b/quasimodo/quasimodo_brain/src/modelserver.cpp @@ -157,20 +157,28 @@ void publish_history(std::vector::Ptr> history } void dumpDatabase(std::string path = "."){ + printf("%i\n",__LINE__); char command [1024]; sprintf(command,"rm -r -f %s/database_tmp",path.c_str()); int r = system(command); + printf("%i\n",__LINE__); sprintf(command,"mkdir %s/database_tmp",path.c_str()); r = system(command); + printf("%i\n",__LINE__); for(unsigned int m = 0; m < modeldatabase->models.size(); m++){ + printf("%i\n",__LINE__); char buf [1024]; sprintf(buf,"%s/database_tmp/model_%08i",path.c_str(),m); sprintf(command,"mkdir -p %s",buf); + printf("%i\n",__LINE__); r = system(command); + printf("%i\n",__LINE__); modeldatabase->models[m]->save(std::string(buf)); + printf("%i\n",__LINE__); } + //exit(0); } void retrievalCallback(const quasimodo_msgs::retrieval_query_result & qr){ @@ -325,6 +333,7 @@ bool addIfPossible(ModelDatabase * database, reglib::Model * model, reglib::Mode } bool addToDB(ModelDatabase * database, reglib::Model * model, bool add){// = true){, bool deleteIfFail = false){ +printf("start: %s\n",__PRETTY_FUNCTION__); if(add){ if(model->submodels.size() > 2){ reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); @@ -348,8 +357,12 @@ bool addToDB(ModelDatabase * database, reglib::Model * model, bool add){// = tru if(show_search){showModels(res);} for(unsigned int i = 0; i < res.size(); i++){ - if(addIfPossible(database,model,res[i])){return true;} + if(addIfPossible(database,model,res[i])){ + printf("stop: %s\n",__PRETTY_FUNCTION__); + return true; + } } +printf("stop: %s\n",__PRETTY_FUNCTION__); return false; } @@ -401,6 +414,7 @@ bool runSearch(ModelDatabase * database, reglib::Model * model, int number_of_se std::set searchset; void addNewModel(reglib::Model * model){ +printf("start: %s\n",__PRETTY_FUNCTION__); reglib::RegistrationRandom * reg = new reglib::RegistrationRandom(); reg->visualizationLvl = show_reg_lvl; reglib::ModelUpdaterBasicFuse * mu = new reglib::ModelUpdaterBasicFuse( model, reg); @@ -428,8 +442,10 @@ void addNewModel(reglib::Model * model){ modeldatabase->add(newmodelHolder); addToDB(modeldatabase, newmodelHolder,false); + printf("%i\n",__LINE__); show_sorted(); + printf("%i\n",__LINE__); bool do_next = true; while(do_next && run_search){ printf("running search loop\n"); @@ -450,9 +466,13 @@ void addNewModel(reglib::Model * model){ } } + printf("%i\n",__LINE__); for(unsigned int i = 0; i < modeldatabase->models.size(); i++){publish_history(modeldatabase->models[i]->getHistory());} + printf("%i\n",__LINE__); publishDatabasePCD(); + printf("%i\n",__LINE__); dumpDatabase(savepath); + printf("stop: %s\n",__PRETTY_FUNCTION__); } void somaCallback(const std_msgs::String & m){printf("somaCallback(%s)\n",m.data.c_str());} @@ -514,6 +534,8 @@ int main(int argc, char **argv){ std::vector soma_input_model_subs; std::vector modelpcds; + bool clearQDB = false; + int inputstate = -1; for(int i = 1; i < argc;i++){ printf("input: %s\n",argv[i]); @@ -542,6 +564,7 @@ int main(int argc, char **argv){ else if(std::string(argv[i]).compare("-show_search") == 0){ printf("show_search\n"); show_search = true;} else if(std::string(argv[i]).compare("-show_modelbuild") == 0){ printf("show_modelbuild\n"); visualization = true; show_modelbuild = true;} else if(std::string(argv[i]).compare("-loadModelsPCDs") == 0){ inputstate = 13;} + else if(std::string(argv[i]).compare("-clearQDB") == 0){ clearQDB = true;} else if(inputstate == 1){ reglib::Camera * cam = reglib::Camera::load(std::string(argv[i])); delete cameras[0]; @@ -590,6 +613,25 @@ int main(int argc, char **argv){ } } +// if(clearQDB){ +// printf("rm -r -f %s\n",(savepath+"/database_tmp").c_str()); +// char command [1024]; +// printf(command,"rm -r -f %s",(savepath+"/database_tmp").c_str()); +// int r = system(command); +// } + + +// if(fileExists(savepath+"/database_tmp")){ +// printf("file exists\n"); + +// }else{ +// printf("making savedi: %s\n",(savepath+"/database_tmp").c_str()); +// char command [1024]; +// sprintf(command,"mkdir %s",(savepath+"/database_tmp").c_str()); +// int r = system(command); +// } +// //exit(0); + if(visualization){ viewer = boost::shared_ptr(new pcl::visualization::PCLVisualizer ("Modelserver Viewer")); viewer->addCoordinateSystem(0.1); diff --git a/quasimodo/quasimodo_models/src/model/Model.cpp b/quasimodo/quasimodo_models/src/model/Model.cpp index 5717143d..3bd032af 100644 --- a/quasimodo/quasimodo_models/src/model/Model.cpp +++ b/quasimodo/quasimodo_models/src/model/Model.cpp @@ -641,6 +641,7 @@ void Model::save(std::string path){ outfile.close(); delete[] buffer; */ +printf("%i\n",__LINE__); char buf [1024]; struct stat sb; @@ -657,25 +658,27 @@ void Model::save(std::string path){ for(unsigned int f = 0; f < frames.size(); f++){ posesfile << relativeposes[f] << endl <save(std::string(buf)); sprintf(buf,"%s/frame_%08i.pcd",path.c_str(),f); frames[f]->savePCD(std::string(buf),relativeposes[f]); - +printf("%i\n",__LINE__); sprintf(buf,"%s/modelmask_%08i.png",path.c_str(),f); cv::imwrite( buf, modelmasks[f]->getMask() ); } posesfile.close(); - +printf("%i\n",__LINE__); ofstream submodels_posesfile; submodels_posesfile.open (path+"/submodels_relativeposes.txt"); for(unsigned int i = 0; i < submodels.size(); i++){ submodels_posesfile << submodels_relativeposes[i] << endl <save(std::string(buf)); + printf("%i\n",__LINE__); } submodels_posesfile.close(); /* From 1923d59489f0a7361e07e8810939cccd8d88dc6a Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 9 Dec 2016 11:38:15 +0100 Subject: [PATCH 254/255] ... --- .../quasimodo_brain/src/metaroom_additional_view_processing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 944a494e..059d0128 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -993,7 +993,7 @@ int processMetaroom(CloudPtr dyncloud, std::string path, bool store_old_xml = tr printf("peopleoverlaps: %i\n",peopleoverlaps); - if(masks.size() > 0){ + if(masks.size() > 0 && cloud_cluster->points.size() > 0){ if(peopleoverlaps == 0 && cloud_cluster->points.size() >= minClusterSize){ if(store_old_xml){ From 768222b23757b33cbd18d70f2faf6d312feda710 Mon Sep 17 00:00:00 2001 From: jekekrantz Date: Fri, 9 Dec 2016 11:55:09 +0100 Subject: [PATCH 255/255] removing exit 0 --- .../src/metaroom_additional_view_processing.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp index 059d0128..cbedbcf3 100644 --- a/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp +++ b/quasimodo/quasimodo_brain/src/metaroom_additional_view_processing.cpp @@ -406,7 +406,6 @@ reglib::Model * processAV(std::string path, bool compute_edges = true, std::stri bgmassreg->stopval = 0.0005; bgmassreg->addModel(sweep); bgmassreg->setData(frames,masks); - // exit(0); //printf("%s::%i\n",__PRETTY_FUNCTION__,__LINE__); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(both_unrefined); @@ -571,7 +570,7 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: unsigned char * rgbdata = (unsigned char *)rgb.data; cv::Mat depth; - depth.create(cloud->height,cloud->width,CV_16UC1); + // exit(0); depth.create(cloud->height,cloud->width,CV_16UC1); unsigned short * depthdata = (unsigned short *)depth.data; unsigned int nr_data = cloud->height * cloud->width; @@ -751,7 +750,7 @@ reglib::Model * getAVMetaroom(std::string path, bool compute_edges = true, std:: startTime_store = reglib::getTime(); reglib::Model * test = quasimodo_brain::getModelFromSegment(*np,soma_id); printf("load time: %6.6f\n",reglib::getTime()-startTime_store); -exit(0); + return fullmodel; } @@ -1280,7 +1279,6 @@ void trainMetaroom(std::string path){ bgmassreg->stopval = 0.0005; bgmassreg->setData(fullmodel->frames,fullmodel->modelmasks); reglib::MassFusionResults bgmfr = bgmassreg->getTransforms(fullmodel->relativeposes); - exit(0); quasimodo_brain::savePoses(overall_folder+"/"+posepath,bgmfr.poses,17); fullmodel->fullDelete(); delete fullmodel; @@ -2080,7 +2078,7 @@ void processSweepForDatabase(std::string path, std::string savePath){ sweep->fullDelete(); delete sweep; - // exit(0); + // int additional_nrviews = 0; // QStringList objectFiles = QDir(sweep_folder.c_str()).entryList(QStringList("*object*.xml")); // for (auto objectFile : objectFiles){ @@ -2387,7 +2385,7 @@ int main(int argc, char** argv){ printf("overall_folder: %s\n",overall_folder.c_str()); // addSceneToLastMetaroom("ed5f22eb-e6c0-426c-b905-4800780ca596"); -//exit(0); + printf("testpaths: %i\n",testpaths.size()); for(unsigned int i = 0; i < testpaths.size(); i++){ testPath(testpaths[i]);