inline bool greater_than_strict(std::multiset< int > multiset1, std::multiset< int > multiset2, unsigned int id1, unsigned int id2){
std::multiset< int >::iterator multiset1_itr = multiset1.begin(), multiset2_itr = multiset2.begin();
int fi,fj;
for ( fi = multiset1.size(),fj = multiset2.size(); fi>0 && fj>0 ;fi--,fj--)
{
if (*multiset1_itr == *multiset2_itr)
{
multiset1_itr++ , multiset2_itr++;
}
else {
//return *multiset1_itr < *multiset2_itr ; //ascending
return *multiset1_itr > *multiset2_itr ;//descending
}
}
//Haven't returned yet and the foor loop exited, so that means the two input multiset have equal entries atleast the common portions
if (fi==fj)
{
//If they are equal, that means the relevance multiset is exactly the same, in that case we need to enforce stricter ordering based on node ids
return id1> id2; //Descending
}
//return (fi<fj);//asecending
return (fi>fj);//descending
};
//回溯法求解
std::multiset<int> knapsack(std::multiset<std::pair<int,int> >& products,int maxLoad){
int N=products.size();
int *w = new int[N+1];
int *v = new int[N+1];
int *IDs = new int[N+1];
w[0]=0;v[0]=0;
int i=1;
for(auto p : products){
w[i]=v[i]=p.second;
IDs[i]=p.first;
i++;
}
int *flag= new int[N+1]; //flag[i][j]表示在容量为j的时候是否将第i件物品放入背包
for (int i = 0; i < N+1; i++) flag[i]=0;
zero_one_pack(maxLoad, w, v, flag, N);
//cout << "需要放入的物品如下" << endl;
multiset<int> sol;
for (int i = 1; i <= N; i++) {
if (flag[i] == 1)
sol.insert(IDs[i]);
//cout << i << "重量为" << w[i] << ", 价值为" << v[i] << endl;
}
//cout << "总的价值为: " << total_value << endl;
delete w;
delete v;
delete flag;
delete IDs;
return sol;
}
/**
A = x1 and x2 and ... xn
create cnf to create an Equility.
* @brief Solver::SatBaseClass::createTseitinOr
* @param vars x1, ..., xn
* @return A
*/
uint Solver::SatBaseClass::createTseitinAnd(std::multiset<literal>& vars){
if(vars.size() == 0) return getTrueVar();
uint A(++countVars);
// if A is true, then all of the xi must be true
result << A << " ";
for(const literal& t: vars){
if(t.positiv){
result << "-" << t.varId << " ";
}else {
result << t.varId << " ";
}
}
result << "0" << std::endl;
countClausel++;
// if one of the xi is false then A is false
for(const literal& t: vars){
result << "-" << A << " ";
if(t.positiv){
result << t.varId << " 0" << std::endl;
} else {
result << "-" << t.varId << " 0" << std::endl;
}
countClausel++;
}
return A;
}
//if return is 0 then neither greater or equal, if returned 1 then equal, if 2 then greater
inline int equal_and_greater(std::multiset< int > multiset1, std::multiset< int > multiset2){
std::multiset< int >::iterator multiset1_itr = multiset1.begin(), multiset2_itr = multiset2.begin();
int fi,fj;
for ( fi = multiset1.size(),fj = multiset2.size(); fi>0 && fj>0 ;fi--,fj--)
{
if (*multiset1_itr == *multiset2_itr)
{
multiset1_itr++ ; multiset2_itr++;
}
else {
return ((*multiset1_itr > *multiset2_itr) ? 2 : (*multiset1_itr == *multiset2_itr)? 1 : 0) ;
}
}
return (fi < fj) ? 0: (fi > fj) ? 2: 1;
};
bool TrajectoryEvaluator::checkTrajectoriesForCollision(const std::multiset<PolyTraj2D, PolyTraj2D::CompPolyTraj2D>& traj_set,
const std::map<int, Vehicle>& predicted_obstacles, const std::string set_name,
std::multiset<PolyTraj2D>::const_iterator& best_traj, double& longest_time_to_collision) {
bool collision = true, found_maybe = false;
double collision_time;
longest_time_to_collision = 0;
std::multiset<PolyTraj2D>::const_iterator it_traj;
uint32_t j = 0;
for (it_traj = traj_set.begin(), j = 0; it_traj != traj_set.end(); it_traj++, j++) {
TrjOccupancyState_t static_collision = TRJ_BLOCKED;
if (obstacle_map_) {
pthread_mutex_lock(&obstacle_map_mutex_);
static_collision = checkCollisionStatic(it_traj->trajectory2D_); // TODO: add collision_time and longest_time_to_collision to static check
pthread_mutex_unlock(&obstacle_map_mutex_);
if (static_collision==TRJ_BLOCKED) {
continue;
}
// std::vector<driving_common::TrajectoryPoint2D>::const_iterator tit = it_traj->trajectory2D_.begin(), tit_end = it_traj->trajectory2D_.end();
// bool zero_velocity=true;
// for(; tit!=tit_end; tit++) {
// if(std::abs((*tit).v)>.01) {zero_velocity=false; break;}
// }
// if(zero_velocity) {continue;}
}
if (!checkCollisionOfTrajectoriesDynamic(it_traj->trajectory2D_, predicted_obstacles, collision_time)) {
if (j != 0) {std::cout << j << "th trajectory (" << set_name << ") of " << traj_set.size() << " is free.\n";}
longest_time_to_collision = std::numeric_limits<double>::infinity();
// we found a collision-free trajectory regardless if it's in maybe range or not
collision = false;
// if we did not find any free before or we found a free now and had a maybe before => update best trajectory
if(!found_maybe || static_collision==TRJ_FREE) {best_traj = it_traj;}
// if there are only maybes we want the one that was found first since it's the best :-)
// otherwise we found a free one and are done
if(static_collision==TRJ_MAYBE_BLOCKED) {found_maybe=true;}
else {
break;
}
}
else {
if (collision_time > longest_time_to_collision) {
longest_time_to_collision = collision_time;
best_traj = it_traj;
}
}
}
return collision;
}
inline bool greater_than(std::multiset< int > multiset1, std::multiset< int > multiset2){
std::multiset< int >::iterator multiset1_itr = multiset1.begin(), multiset2_itr = multiset2.begin();
int fi,fj;
for ( fi = multiset1.size(),fj = multiset2.size(); fi>0 && fj>0 ;fi--,fj--)
{
if (*multiset1_itr == *multiset2_itr)
{
multiset1_itr++ , multiset2_itr++;
}
else {
//return *multiset1_itr < *multiset2_itr ; //ascending
return *multiset1_itr > *multiset2_itr ;//descending
}
}
//Haven't returned yet and the foor loop exited, so that means the two input multiset have equal entries atleast the common portions
//return (fi<fj);//asecending
return (fi>fj);//descending
};
void operator()(clmdep_msgpack::object::with_zone& o, const std::multiset<T, Compare, Alloc>& v) const {
o.type = clmdep_msgpack::type::ARRAY;
if (v.empty()) {
o.via.array.ptr = nullptr;
o.via.array.size = 0;
} else {
uint32_t size = checked_get_container_size(v.size());
clmdep_msgpack::object* p = static_cast<clmdep_msgpack::object*>(o.zone.allocate_align(sizeof(clmdep_msgpack::object)*size));
clmdep_msgpack::object* const pend = p + size;
o.via.array.ptr = p;
o.via.array.size = size;
typename std::multiset<T, Compare, Alloc>::const_iterator it(v.begin());
do {
*p = clmdep_msgpack::object(*it, o.zone);
++p;
++it;
} while(p < pend);
}
}
inline std::string PrettyPrint(const std::multiset<T>& settoprint, const bool add_delimiters=false, const std::string& separator=", ")
{
std::ostringstream strm;
if (settoprint.size() > 0)
{
if (add_delimiters)
{
strm << "(";
typename std::multiset<T, Compare, Allocator>::const_iterator itr;
for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
{
if (itr != settoprint.begin())
{
strm << separator << PrettyPrint(*itr, add_delimiters, separator);
}
else
{
strm << PrettyPrint(*itr, add_delimiters, separator);
}
}
strm << ")";
}
else
{
typename std::multiset<T, Compare, Allocator>::const_iterator itr;
for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
{
if (itr != settoprint.begin())
{
strm << separator << PrettyPrint(*itr, add_delimiters, separator);
}
else
{
strm << PrettyPrint(*itr, add_delimiters, separator);
}
}
}
}
return strm.str();
}
int main()
{
{
typedef int V;
V ar[] =
{
1,
1,
1,
2,
2,
2,
3,
3,
3,
4,
4,
4,
5,
5,
5,
6,
6,
6,
7,
7,
7,
8,
8,
8
};
std::multiset<int> m(ar, ar+sizeof(ar)/sizeof(ar[0]));
assert(std::distance(m.begin(), m.end()) == m.size());
assert(std::distance(m.rbegin(), m.rend()) == m.size());
std::multiset<int>::iterator i;
i = m.begin();
std::multiset<int>::const_iterator k = i;
assert(i == k);
for (int j = 1; j <= 8; ++j)
for (int k = 0; k < 3; ++k, ++i)
assert(*i == j);
}
{
typedef int V;
V ar[] =
{
1,
1,
1,
2,
2,
2,
3,
3,
3,
4,
4,
4,
5,
5,
5,
6,
6,
6,
7,
7,
7,
8,
8,
8
};
const std::multiset<int> m(ar, ar+sizeof(ar)/sizeof(ar[0]));
assert(std::distance(m.begin(), m.end()) == m.size());
assert(std::distance(m.cbegin(), m.cend()) == m.size());
assert(std::distance(m.rbegin(), m.rend()) == m.size());
assert(std::distance(m.crbegin(), m.crend()) == m.size());
std::multiset<int, double>::const_iterator i;
i = m.begin();
for (int j = 1; j <= 8; ++j)
for (int k = 0; k < 3; ++k, ++i)
assert(*i == j);
}
}
inline BOSTREAM2(const std::multiset<T, C, A> &a) { return itwrite(out, a.size(), a.begin()); }
void ArithmeticOperationsTest()
{
#if !(SIG_MSVC_VER <= 120)
// スカラー + スカラー
assert(plus(1, 1.0) == 2.0);
#endif
// ベクトル + スカラー
for_each(
DebugEqual(),
data2 + 1.0,
zipWith(std::plus<double>(), data2, replicate(data2.size(), 1))
);
for_each(
DebugEqual(),
plus(data2, 1.0),
zipWith(std::plus<double>(), data2, replicate(data2.size(), 1))
);
// スカラー + ベクトル
for_each(
DebugEqual(),
1.0 + data3,
zipWith(std::plus<double>(), replicate(data3.size(), 1), data3)
);
for_each(
DebugEqual(),
plus(1.0, data3),
zipWith(std::plus<double>(), replicate(data3.size(), 1), data3)
);
// ベクトル(sequence) + ベクトル(sequence)
for_each(
DebugEqual(),
data1 + data2,
zipWith(std::plus<double>(), data1, data2)
);
for_each(
DebugEqual(),
plus(data1, data2),
zipWith(std::plus<double>(), data1, data2)
);
// ベクトル(set) + ベクトル(static array)
for_each(
DebugEqual(),
data3 + data0,
zipWith(std::plus<double>(), data3, data0)
);
for_each(
DebugEqual(),
plus(data3, data0),
zipWith(std::plus<double>(), data3, data0)
);
// ベクトル(hash set) + ベクトル(static array)
assert(foldl(std::plus<double>(), 0.0, data4 + data0) == foldl(std::plus<double>(), 0.0, zipWith(std::plus<double>(), data4, data0)));
assert(foldl(std::plus<double>(), 0.0, plus(data4, data0)) == foldl(std::plus<double>(), 0.0, zipWith(std::plus<double>(), data4, data0)));
// 減算
#if !(SIG_MSVC_VER <= 120)
assert(minus(2, 1.0) == 1.0);
assert(minus(1.0, 2) == -1.0);
#endif
for_each(
DebugEqual(),
data3 - 1.0,
zipWith(std::minus<double>(), data3, replicate(data3.size(), 1))
);
for_each(
DebugEqual(),
minus(data3, 1.0),
zipWith(std::minus<double>(), data3, replicate(data3.size(), 1))
);
for_each(
DebugEqual(),
1.0 - data1,
zipWith(std::minus<double>(), replicate(data1.size(), 1), data1)
);
for_each(
DebugEqual(),
minus(1.0, data1),
zipWith(std::minus<double>(), replicate(data1.size(), 1), data1)
);
for_each(
DebugEqual(),
data2 - data0,
zipWith(std::minus<double>(), data2, data0)
);
for_each(
DebugEqual(),
minus(data2, data0),
zipWith(std::minus<double>(), data2, data0)
);
// 乗算
#if !(SIG_MSVC_VER <= 120)
assert(mult(2, 2.0) == 4.0);
#endif
for_each(
DebugEqual(),
data3 * 2.5,
zipWith(std::multiplies<double>(), data3, replicate(data3.size(), 2.5))
);
for_each(
DebugEqual(),
mult(data3, 2.5),
zipWith(std::multiplies<double>(), data3, replicate(data3.size(), 2.5))
);
for_each(
DebugEqual(),
2.5 * data1,
zipWith(std::multiplies<double>(), replicate(data1.size(), 2.5), data1)
);
for_each(
DebugEqual(),
mult(2.5, data1),
zipWith(std::multiplies<double>(), replicate(data1.size(), 2.5), data1)
);
for_each(
DebugEqual(),
data2 * data0,
zipWith(std::multiplies<double>(), data2, data0)
);
for_each(
DebugEqual(),
mult(data2, data0),
zipWith(std::multiplies<double>(), data2, data0)
);
// 除算
#if !(SIG_MSVC_VER <= 120)
assert(div(2, 1.0) == 2.0);
assert(div(1.0, 2) == 0.5);
#endif
for_each(
DebugEqual(),
data3 / 3.0,
zipWith(std::divides<double>(), data3, replicate(data2.size(), 3.0))
);
for_each(
DebugEqual(),
div(data3, 3.0),
zipWith(std::divides<double>(), data3, replicate(data2.size(), 3.0))
);
for_each(
DebugEqual(),
3.0 / data1,
zipWith(std::divides<double>(), replicate(data1.size(), 3.0), data1)
);
for_each(
DebugEqual(),
div(3.0, data1),
zipWith(std::divides<double>(), replicate(data1.size(), 3.0), data1)
);
for_each(
DebugEqual(),
data2 / data0,
zipWith(std::divides<double>(), data2, data0)
);
for_each(
DebugEqual(),
div(data2, data0),
zipWith(std::divides<double>(), data2, data0)
);
auto rr = 2 / data2 + 2 * data1 - data0 * 0.5;
}
void GridFitter::visualizeDebug(std::multiset<candidate_t> const& grids, const cv::Mat& roi, const cv::Size2i roiSize, const cv::Mat& edgeRoiX, const cv::Mat& edgeRoiY, const Tag &tag, cv::Mat const& binarizedROI, std::string winName, const size_t numBest)
{
cv::Mat binarizedROICpy;
cv::cvtColor(binarizedROI, binarizedROICpy, CV_GRAY2BGR);
cv::Mat edgeX;
cv::cvtColor(edgeRoiX, edgeX, CV_GRAY2BGR);
cv::Mat edgeY;
cv::cvtColor(edgeRoiY, edgeY, CV_GRAY2BGR);
cv::Mat roiCpy;
cv::cvtColor(roi, roiCpy, CV_GRAY2BGR);
const size_t to = std::min(grids.size(), numBest);
size_t idx = 0;
for (candidate_t const& candidate : grids) {
std::vector<cv::Mat> images;
PipelineGrid grid(candidate.config);
images.push_back(tag.getRepresentations().orig);
images.push_back(roiCpy);
cv::Mat origCopy;
roiCpy.copyTo(origCopy);
pipeline::Ellipse const& ell = tag.getCandidatesConst().front().getEllipse();
cv::ellipse(origCopy, ell.getCen(), ell.getAxis(), ell.getAngle(), 0, 360, cv::Scalar(0, 255, 0), 2);
images.push_back(origCopy);
images.push_back(edgeX);
images.push_back(edgeY);
images.push_back(binarizedROICpy);
images.push_back(grid.getProjectedImage(roiSize));
cv::Mat blendedBin;
cv::addWeighted(binarizedROICpy, 0.6, grid.getProjectedImage(roiSize), 0.4, 0.0, blendedBin);
images.push_back(blendedBin);
cv::Mat blended;
cv::addWeighted(tag.getRepresentations().orig, 0.8, grid.getProjectedImage(roiSize), 0.2, 0.0, blended);
images.push_back(blended);
cv::Mat cannyBlendedX;
edgeX.copyTo(cannyBlendedX);
grid.drawContours(cannyBlendedX, 0.9, cv::Vec3b(150, 200, 170));
images.push_back(cannyBlendedX);
cv::Mat cannyBlendedY;
edgeY.copyTo(cannyBlendedY);
grid.drawContours(cannyBlendedY, 0.9, cv::Vec3b(150, 200, 170));
images.push_back(cannyBlendedY);
cv::Mat origCopyOverlay;
roiCpy.copyTo(origCopyOverlay);
grid.drawContours(origCopyOverlay, 0.5);
images.push_back(origCopyOverlay);
const auto canvas = CvHelper::makeCanvas(images, images[0].rows + 10, 1);
std::string title(winName + " " + std::to_string(idx) + " (error: " + std::to_string(candidate.error) + ")");
cv::namedWindow(title);
cv::imshow(title, canvas);
++idx;
if (idx == to) break;
}
if (!grids.empty()) {
bool cont = true;
while (cont) {
const char c = cv::waitKey();
if (c == 'd') {
cv::destroyAllWindows();
cont = false;
} else if (c == 'c') {
cont = false;
}
}
}
}