测试了std::vector的交集求解,并对比里std

本文通过对比自定义函数与标准库函数在处理大量数据时的时间效率,展示了如何通过实现细节优化自定义函数,从而达到更高的执行效率。包括测试用例设计、函数实现与结果分析。



需要事先引入的头文件:

#include <iostream>

#include <set>
#include <vector>
#include <algorithm>
#include <iterator>



自定义类:

class Loc
{
public:
	short c;
	short r;

	// ...省略没有必要的

	// std::less (这样就可以用一些算法了)
	bool operator < (const Loc& right)
	{
		return ((this->c * this->c + this->r * this->r) < (right.c * right.c + right.r * right.r)) ? true : false;
	}

};




// 测试时间效率
void test_std_vector_intersection_tm_small()
{

	std::vector<Loc> v1 = { Loc(1, 1), Loc(1, 2), Loc(1, 3), Loc(1, 4), Loc(1, 5) };
	std::vector<Loc> v2 = { Loc(2, 1), Loc(1, 2), Loc(1, 3), Loc(2, 4), Loc(1, 5) };

	std::sort(v1.begin(), v1.end());
	std::sort(v2.begin(), v2.end());

	std::vector<Loc> v_intersection;
	TM_INIT;
	TM_START;
	for (int i = 0; i < 300000; ++i)
	{
		std::set_intersection(
			v1.begin(), v1.end(),
			v2.begin(), v2.end(),
			std::back_inserter(v_intersection));
		v_intersection.clear();
	}
	TM_END("1");
	// 3.0s

	TM_START;
	for (int i = 0; i < 300000; ++i)
	{
		calc_intersection_small(v1, v2, v_intersection);
		v_intersection.clear();
	}
	TM_END("2");


	// r= 100000 [1]2.0 [2]1.0;
	// r= 300000 [1]7.0 [2]4.0;
	// r= 300000 [1]7.0 [2]3.0;
	// 结论: 自己写的函数,效率高于通用函数


	// r= 300000 ([1]加入sort) [1]7.0 [2]3.0;
	for (Loc n : v_intersection)
		std::cout << "(" << n.c << ' ' << n.r << ") ";

	// 5, 7
}


// 测试大量数据
void test_std_vector_intersection_tm_big()
{

	std::vector<Loc> v1 = { 
		{ 1, 1 }, { 1, 2 }, { 1, 3 }, { 1, 4 }, { 1, 5 }, { 1, 6 }, { 1, 7 }, { 1, 8 }, { 1, 9 }, 
		{ 2, 1 }, { 2, 2 }, { 2, 3 }, { 2, 4 }, { 2, 5 }, { 2, 6 }, { 2, 7 }, { 2, 8 }, { 2, 9 },
		{ 3, 1 }, { 3, 2 }, { 3, 3 }, { 3, 4 }, { 3, 5 }, { 3, 6 }, { 3, 7 }, { 3, 8 }, { 3, 9 },
		{ 4, 1 }, { 4, 2 }, { 4, 3 }, { 4, 4 }, { 4, 5 }, { 4, 6 }, { 4, 7 }, { 4, 8 }, { 4, 9 },
		{ 5, 1 }, { 5, 2 }, { 5, 3 }, { 5, 4 }, { 5, 5 }, { 5, 6 }, { 5, 7 }, { 5, 8 }, { 5, 9 },
		{ 6, 1 }, { 6, 2 }, { 6, 3 }, { 6, 4 }, { 6, 5 }, { 6, 6 }, { 6, 7 }, { 6, 8 }, { 6, 9 },
		{ 7, 1 }, { 7, 2 }, { 7, 3 }, { 7, 4 }, { 7, 5 }, { 7, 6 }, { 7, 7 }, { 7, 8 }, { 7, 9 },
		{ 8, 1 }, { 8, 2 }, { 8, 3 }, { 8, 4 }, { 8, 5 }, { 8, 6 }, { 8, 7 }, { 8, 8 }, { 8, 9 },
		{ 9, 1 }, { 9, 2 }, { 9, 3 }, { 9, 4 }, { 9, 5 }, { 9, 6 }, { 9, 7 }, { 9, 8 }, { 9, 9 },
		{ 0, 1 }, { 0, 2 }, { 0, 3 }, { 0, 4 }, { 0, 5 }, { 0, 6 }, { 0, 7 }, { 0, 8 }, { 0, 9 },
	};

	// \ 对角线被修改为 x,0 一共有9个不同
	std::vector<Loc> v2 = {
		{ 1, 0 }, { 1, 2 }, { 1, 3 }, { 1, 4 }, { 1, 5 }, { 1, 6 }, { 1, 7 }, { 1, 8 }, { 1, 9 },
		{ 2, 1 }, { 2, 0 }, { 2, 3 }, { 2, 4 }, { 2, 5 }, { 2, 6 }, { 2, 7 }, { 2, 8 }, { 2, 9 },
		{ 3, 1 }, { 3, 2 }, { 3, 0 }, { 3, 4 }, { 3, 5 }, { 3, 6 }, { 3, 7 }, { 3, 8 }, { 3, 9 },
		{ 4, 1 }, { 4, 2 }, { 4, 3 }, { 4, 0 }, { 4, 5 }, { 4, 6 }, { 4, 7 }, { 4, 8 }, { 4, 9 },
		{ 5, 1 }, { 5, 2 }, { 5, 3 }, { 5, 4 }, { 5, 0 }, { 5, 6 }, { 5, 7 }, { 5, 8 }, { 5, 9 },
		{ 6, 1 }, { 6, 2 }, { 6, 3 }, { 6, 4 }, { 6, 5 }, { 6, 0 }, { 6, 7 }, { 6, 8 }, { 6, 9 },
		{ 7, 1 }, { 7, 2 }, { 7, 3 }, { 7, 4 }, { 7, 5 }, { 7, 6 }, { 7, 0 }, { 7, 8 }, { 7, 9 },
		{ 8, 1 }, { 8, 2 }, { 8, 3 }, { 8, 4 }, { 8, 5 }, { 8, 6 }, { 8, 7 }, { 8, 0 }, { 8, 9 },
		{ 9, 1 }, { 9, 2 }, { 9, 3 }, { 9, 4 }, { 9, 5 }, { 9, 6 }, { 9, 7 }, { 9, 8 }, { 9, 0 },
		{ 0, 1 }, { 0, 2 }, { 0, 3 }, { 0, 4 }, { 0, 5 }, { 0, 6 }, { 0, 7 }, { 0, 8 }, { 0, 9 },
	};

	std::sort(v1.begin(), v1.end());
	std::sort(v2.begin(), v2.end());

	std::vector<Loc> v_intersection;
	TM_INIT;
	TM_START;
	for (int i = 0; i < 100000; ++i)
	{
		std::set_intersection(
			v1.begin(), v1.end(),
			v2.begin(), v2.end(),
			std::back_inserter(v_intersection));
		v_intersection.clear();
	}
	TM_END("1");
	// 3.0s

	TM_START;
	for (int i = 0; i < 100000; ++i)
	{
		calc_intersection_small(v1, v2, v_intersection);
		v_intersection.clear();
	}
	TM_END("2");

	TM_START;
	for (int i = 0; i < 100000; ++i)
	{
		calc_intersection_big(v1, v2, v_intersection);
		v_intersection.clear();
	}
	TM_END("3");

	// r= 100000 [1]21.0 [2]14.0 [3]63.0
	// 结论: 自己写的函数,效率高于通用函数


	// r= 300000 ([1]加入sort) [1]x.0 [2]x.0;
	for (Loc n : v_intersection)
		std::cout << "(" << n.c << ' ' << n.r << ") ";

}


一些自己实现的函数:

void calc_intersection_small(const std::vector<Loc>& v1, const std::vector<Loc>& v2, std::vector<Loc>& v_out)
{
	bool isFind = false;
	for (auto it1 = v1.begin(); it1 != v1.end(); ++it1)
	{
		for (auto it2 = v2.begin(); it2 != v2.end(); ++it2)
		{
			if ((*it1) == (*it2))
			{
				isFind = true;
				v_out.push_back(*it1);
			}
			if (isFind)
				break;
		}
		if (isFind)
		{
			continue;
		}
	}
}

void calc_intersection_big(const std::vector<Loc>& v1, const std::vector<Loc>& v2, std::vector<Loc>& v_out)
{
	std::vector<Loc> vv1;
	std::vector<Loc> vv2;

	vv1.insert(vv1.begin(), v1.begin(), v1.end());
	vv2.insert(vv2.begin(), v2.begin(), v2.end());

	bool isFind = false;
	for (auto it1 = vv1.begin(); it1 != vv1.end(); )
	{
		for (auto it2 = vv2.begin(); it2 != vv2.end(); )
		{
			if ((*it1) == (*it2))
			{
				isFind = true;
				v_out.push_back(*it1);
			}
			else
				++it2;

			if (isFind)
			{
				// it2 = vv2.erase(it2);
				break;
			}
		}

		if (isFind)
		{
			it1 = vv1.erase(it1);
			continue;
		}
		else
			++it1;
	}
}





TRAC_IK::TRAC_IK(const std::string& base_link, const std::string& tip_link, const std::string& URDF_param, double _maxtime, double _eps, SolveType _type) : initialized(false), eps(_eps), maxtime(_maxtime), solvetype(_type) { urdf::Model robot_model; robot_model.initString(URDF_param); KDL::Tree tree; if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) std::cerr << "Failed to extract kdl tree from xml robot description"; if (!tree.getChain(base_link, tip_link, chain)) std::cerr << "Couldn't find chain " << base_link.c_str() << " to " << tip_link.c_str(); std::vector<KDL::Segment> chain_segs = chain.segments; urdf::JointConstSharedPtr joint; std::vector<double> l_bounds, u_bounds; lb.resize(chain.getNrOfJoints()); ub.resize(chain.getNrOfJoints()); uint joint_num = 0; for (unsigned int i = 0; i < chain_segs.size(); ++i) { joint = robot_model.getJoint(chain_segs[i].getJoint().getName()); if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) { joint_num++; float lower, upper; int hasLimits; if (joint->type != urdf::Joint::CONTINUOUS) { if (joint->safety) { lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit); upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit); } else { lower = joint->limits->lower; upper = joint->limits->upper; } hasLimits = 1; } else { hasLimits = 0; } if (hasLimits) { lb(joint_num - 1) = lower; ub(joint_num - 1) = upper; } else { lb(joint_num - 1) = std::numeric_limits<float>::lowest(); ub(joint_num - 1) = std::numeric_limits<float>::max(); } // ROS_DEBUG_STREAM_NAMED("trac_ik", "IK Using joint " << joint->name << " " << lb(joint_num - 1) << " " << ub(joint_num - 1)); } } initialize(); }通俗易懂地解释上述代码作用及含义
09-17
#include <vector> #include <Eigen/Dense> // 需要安装Eigen库 #include<iostream> #include <fstream> #include <stdexcept> // 定义RPB参数结构体(以某卫星格式为例) struct RPBParams { double line_num_coeff[20]; // 行分子多项式系数 double line_den_coeff[20]; // 行分母多项式系数 double samp_num_coeff[20]; // 列分子多项式系数 double samp_den_coeff[20]; // 列分母系数 }; // 地面控制点结构体 struct GCP { double lat, lon, height; // 地理坐标 double x1, y1; // 影像1像素坐标 double x2, y2; // 影像2像素坐标 }; // 计算三次多项式值 P = c0 + c1*X + c2*Y + c3*Z + ... (共20项) double evaluatePoly(const double coeff[20], double lat, double lon, double height) { double X = lat, Y = lon, Z = height; return coeff[0] + coeff[1] * X + coeff[2] * Y + coeff[3] * Z + coeff[4] * X * Y + coeff[5] * X * Z + coeff[6] * Y * Z + coeff[7] * X * X + coeff[8] * Y * Y + coeff[9] * Z * Z + coeff[10] * X * X * X + coeff[11] * Y * Y * Y + coeff[12] * Z * Z * Z + coeff[13] * X * X * Y + coeff[14] * X * X * Z + coeff[15] * Y * Y * X + coeff[16] * Y * Y * Z + coeff[17] * Z * Z * X + coeff[18] * Z * Z * Y + coeff[19] * X * Y * Z; } // 计算单个GCP在两影像中的像素坐标 void computeGCPPixel(const RPBParams& rpb1, const RPBParams& rpb2, GCP& point) { // 影像1的行列计算 double row_num = evaluatePoly(rpb1.line_num_coeff, point.lat, point.lon, point.height); double row_den = evaluatePoly(rpb1.line_den_coeff, point.lat, point.lon, point.height); point.y1 = row_num / row_den; double col_num = evaluatePoly(rpb1.samp_num_coeff, point.lat, point.lon, point.height); double col_den = evaluatePoly(rpb1.samp_den_coeff, point.lat, point.lon, point.height); point.x1 = col_num / col_den; // 影像2同理 // ... (重复上述步骤计算x2, y2) } struct TransformParams { double scale; // 缩放因子 double rotation; // 旋转角(弧度) double tx, ty; // 平移量 }; RPBParams loadRPB(const std::string& filename) { std::ifstream file(filename); if (!file.is_open()) { throw std::runtime_error("无法打开文件: " + filename); } RPBParams params; std::vector<double*> coefficient_blocks = { params.line_num_coeff, params.line_den_coeff, params.samp_num_coeff, params.samp_den_coeff }; for (auto& coeff : coefficient_blocks) { for (int i = 0; i < 20; ++i) { if (!(file >> coeff[i])) { throw std::runtime_error("文件格式错误或数据不足: " + filename); } } } return params; } TransformParams estimateSimilarity(const std::vector<GCP>& gcps) { Eigen::MatrixXd A(gcps.size() * 2, 4); Eigen::VectorXd B(gcps.size() * 2); // 构建线性方程组 AX = B for (size_t i = 0; i < gcps.size(); ++i) { const GCP& p = gcps[i]; // 第一行方程:x2 = a*x1 - b*y1 + tx A(2 * i, 0) = p.x1; A(2 * i, 1) = -p.y1; A(2 * i, 2) = 1; A(2 * i, 3) = 0; B(2 * i) = p.x2; // 第二行方程:y2 = b*x1 + a*y1 + ty A(2 * i + 1, 0) = p.y1; A(2 * i + 1, 1) = p.x1; A(2 * i + 1, 2) = 0; A(2 * i + 1, 3) = 1; B(2 * i + 1) = p.y2; } // 求解最小二乘解 Eigen::Vector4d X = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B); // 提取参数 TransformParams params; params.scale = sqrt(X(0) * X(0) + X(1) * X(1)); params.rotation = atan2(X(1), X(0)); params.tx = X(2); params.ty = X(3); return params; } int main() { // 加载两影像的RPB参数 RPBParams rpb1 = loadRPB("image1.rpb"); RPBParams rpb2 = loadRPB("image2.rpb"); // 生成GCPs(示例为均匀采样) std::vector<GCP> gcps; for (double lat = min_lat; lat <= max_lat; lat += step) { for (double lon = min_lon; lon <= max_lon; lon += step) { GCP p; p.lat = lat; p.lon = lon; p.height = 100.0; // 假设固定高程 computeGCPPixel(rpb1, rpb2, p); gcps.push_back(p); } } // 估计变换参数 TransformParams params = estimateSimilarity(gcps); // 输出结果 std::cout << "缩放因子: " << params.scale << std::endl; std::cout << "旋转角度: " << params.rotation * 180 / M_PI << "度" << std::endl; std::cout << "平移量(tx, ty): (" << params.tx << ", " << params.ty << ")" << std::endl; return 0; }这面的min_lat,max_lat,step,min_lon,max_lon是怎么得出的,用C++实现
04-01
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值