vector<pair<int, Vector3d>> DataGenerator::getImage()
{
vector<pair<int, Vector3d>> image;
Vector3d position = getPosition();
Matrix3d quat = getRotation(); //R_gb
printf("max: %d\n", current_id);
for (int i = 0; i < NUM_POINTS; i++)
{
double xx = pts[i * 3 + 0] - position(0);
double yy = pts[i * 3 + 1] - position(1);
double zz = pts[i * 3 + 2] - position(2);
Vector3d local_point = Ric.inverse() * (quat.inverse() * Vector3d(xx, yy, zz) - Tic);
xx = local_point(0);
yy = local_point(1);
zz = local_point(2);
if (std::fabs(atan2(xx, zz)) <= PI * FOV / 2 / 180
&& std::fabs(atan2(yy, zz)) <= PI * FOV / 2 / 180
&& zz > 0)
{
int n_id = before_feature_id.find(i) == before_feature_id.end() ?
current_id++ : before_feature_id[i];
//#if WITH_NOISE
// Vector3d disturb = Vector3d(distribution(generator) * sqrt(pts_cov(0, 0)) / zz / zz,
// distribution(generator) * sqrt(pts_cov(1, 1)) / zz / zz,
// 0
// );
// image.push_back(make_pair(n_id, disturb + Vector3d(xx / zz, yy / zz, 1)));
//#else
image.push_back(make_pair(n_id, Vector3d(xx, yy, zz)));
printf ("id %d, (%d %d %d)\n", n_id,
pts[i * 3 + 0] ,
pts[i * 3 + 1] ,
pts[i * 3 + 2]
);
//#endif
current_feature_id[i] = n_id;
}
}
before_feature_id = current_feature_id;
current_feature_id.clear();
//sort(image.begin(), image.end(), [](const pair<int, Vector3d> &a,
// const pair<int, Vector3d> &b)
//{
// return a.first < b.first;
//});
return image;
}
// TODO fix up
float computeCost(const Patch &candidate, const Patch &target, const cgra::ivec2 &target_offset, const Heightmap *output,
float feature_cost_weight, float angle_cost_weight, float /*noise_cost_weight*/, float overlap_cost_weight) {
float cost = 0;
// Calculate cost for same non-zero degree patches
if (candidate.degree == target.degree && candidate.degree > 0) {
// Feature Dissimilarity Cost (SSD height profile)
// cout << "feature_cost : " << feature_cost_weight * nssdProfiles(candidate, target) << endl;
cost += feature_cost_weight * nssdProfiles(candidate, target);
// Angle Difference Cost (SSD angle difference)
// cout << "angle_cost : " << angle_cost_weight * ssdOutpaths(candidate, target) << endl;
cost += angle_cost_weight * ssdOutpaths(candidate, target);
}
// Noise Variance Cost (SSD gaussian noise variance difference)
// TODO
// Overlap Cost (SSD overlapping pixels)
float overlap_cost = 0;
int overlap_count = 0;
for (int y = 0; y < int(candidate.data.height()); ++y) {
for (int x = 0; x < int(candidate.data.width()); ++x) {
cgra::ivec2 local_point(x, y);
cgra::ivec2 global_point = target_offset + local_point;
if (!candidate.data.isnan(local_point) && output->inBounds(global_point) && !output->isnan(global_point)) {
float d = candidate.data.at(local_point) - output->at(global_point);
overlap_cost += d * d;
++overlap_count;
}
}
}
if (overlap_count) overlap_cost = overlap_cost / overlap_count;
// cout << "overlap_cost : " << overlap_cost_weight * overlap_cost << endl;
cost += overlap_cost_weight * overlap_cost;
return cost;
}
