C++ mrpt类代码示例

int main()
{
	try
	{
		printf ("Starting publisher...\n");

		void *context = zmq_ctx_new ();
		void *pub_sock = zmq_socket (context, ZMQ_PUB);
		int rc = zmq_bind (pub_sock, "tcp://*:5555");
		assert (rc == 0);

		while (1)
		{
			mrpt::poses::CPose3D  my_pose(0.5f,0.5f,1.5f ,DEG2RAD(-90.0f),DEG2RAD(0),DEG2RAD(-90.0f)  );
			printf("Publishing pose...\n");
			mrpt::comms::mrpt_send_to_zmq(pub_sock, my_pose);
			std::this_thread::sleep_for(100ms);

			mrpt::img::CImage my_img(800,600, CH_RGB);
			printf("Publishing img...\n");
			mrpt::comms::mrpt_send_to_zmq(pub_sock, my_img, 0 /* max_packet_len: 0=no max size */);
			std::this_thread::sleep_for(100ms);
		}


		zmq_close (pub_sock);
		zmq_ctx_destroy (context);
		return 0;
	} catch (std::exception &e)
	{
		std::cerr << "**Exception**: " << e.what() << std::endl;
		return -1;
	}
}

TEST(CPose3DInterpolator, interp)
{
	using namespace mrpt::poses;
	using mrpt::math::TPose3D;
	using mrpt::math::CMatrixDouble44;
	using mrpt::DEG2RAD;

	const mrpt::system::TTimeStamp t0 = mrpt::system::now();
	const mrpt::system::TTimeStamp dt = mrpt::system::secondsToTimestamp(0.10);

	CPose3DInterpolator pose_path;

	pose_path.insert(
		t0, TPose3D(1., 2., 3., DEG2RAD(30.0), DEG2RAD(.0), DEG2RAD(.0)));
	pose_path.insert(
		t0 + 2 * dt, TPose3D(
						 1. + 3., 2. + 4., 3. + 5., DEG2RAD(30.0 + 20.0),
						 DEG2RAD(.0), DEG2RAD(.0)));

	TPose3D interp;
	bool valid;
	pose_path.interpolate(t0 + dt, interp, valid);

	EXPECT_TRUE(valid);
	const TPose3D interp_good(
		1. + 1.5, 2. + 2.0, 3. + 2.5, DEG2RAD(30.0 + 10.0), DEG2RAD(.0),
		DEG2RAD(.0));
	EXPECT_NEAR(
		.0, (CPose3D(interp_good).getHomogeneousMatrixVal<CMatrixDouble44>() -
			 CPose3D(interp).getHomogeneousMatrixVal<CMatrixDouble44>())
				.array()
				.abs()
				.sum(),
		1e-4);
}

// ------------------------------------------------------
//				TestCapture
// ------------------------------------------------------
void TestExtractFeaturesTile()
{
	CDisplayWindow		wind1,wind2;
	CFeatureExtraction	fExt;
	CFeatureList		featsHarris;
	CImage				img;

	string the_img = myDataDir+string("test_image.jpg");

	if (!img.loadFromFile(the_img ))
	{
		cerr << "Cannot load " << the_img  << endl;
		return;
	}
	cout << "Loaded test image: " << the_img << endl;

	CTicTac	tictac;

	cout << "Extracting Harris features (tiled)... [f_harris_tiled.txt]";

	fExt.options.featsType = featHarris;
	fExt.options.harrisOptions.tile_image = true;

	tictac.Tic();
	fExt.detectFeatures( img, featsHarris );
	cout << format("  %.03fms",tictac.Tac()*1000) << endl;

	cout << "Detected " << featsHarris.size() << " features in " << endl;
	featsHarris.saveToTextFile("f_harris_tiled.txt");
	wind1.setWindowTitle("Harris detected features (Tiled image)");
	wind1.showTiledImageAndPoints( img, featsHarris );

	cout << "Extracting Harris features... [f_harris.txt]";

	fExt.options.harrisOptions.tile_image = false;

	tictac.Tic();
	fExt.detectFeatures( img, featsHarris );
	cout << format("  %.03fms",tictac.Tac()*1000) << endl;

	featsHarris.saveToTextFile("f_harris.txt");
	wind2.setWindowTitle("Harris detected features");
	wind2.showTiledImageAndPoints( img, featsHarris );

	mrpt::system::pause();

	return;
}

/*---------------------------------------------------------------
					read_uint64_t
 ---------------------------------------------------------------*/
uint64_t CConfigFileBase::read_uint64_t(
	const std::string& section, const std::string& name, uint64_t defaultValue,
	bool failIfNotFound) const
{
	string s = readString(
		section, name, format("%lu", (long unsigned int)defaultValue),
		failIfNotFound);
	return mrpt::system::os::_strtoull(s.c_str(), nullptr, 0);
}

TEST(CPose3DInterpolator, interp)
{
	using namespace mrpt::poses;
	using mrpt::DEG2RAD;
	using mrpt::math::CMatrixDouble44;
	using mrpt::math::TPose3D;

	auto t0 = mrpt::Clock::now();
	mrpt::Clock::duration dt(std::chrono::milliseconds(100));

	CPose3DInterpolator pose_path;

	pose_path.insert(
		t0, TPose3D(1., 2., 3., DEG2RAD(30.0), DEG2RAD(.0), DEG2RAD(.0)));
	pose_path.insert(
		t0 + 2 * dt, TPose3D(
						 1. + 3., 2. + 4., 3. + 5., DEG2RAD(30.0 + 20.0),
						 DEG2RAD(.0), DEG2RAD(.0)));

	TPose3D interp;
	bool valid;
	pose_path.interpolate(t0 + dt, interp, valid);

	EXPECT_TRUE(valid);
	const TPose3D interp_good(
		1. + 1.5, 2. + 2.0, 3. + 2.5, DEG2RAD(30.0 + 10.0), DEG2RAD(.0),
		DEG2RAD(.0));
	EXPECT_NEAR(
		.0,
		(CPose3D(interp_good).getHomogeneousMatrixVal<CMatrixDouble44>() -
		 CPose3D(interp).getHomogeneousMatrixVal<CMatrixDouble44>())
			.array()
			.abs()
			.sum(),
		2e-4);
}

// ------------------------------------------------------
//				TestExtractFeatures
// ------------------------------------------------------
void TestExtractFeatures()
{
	CDisplayWindow		wind1,wind2,wind3,wind4,wind5;
	CFeatureExtraction	fExt;
	CFeatureList		featsHarris, featsKLT, featsSIFT_Hess, featsSIFT_Lowe, featsSIFT_Vedaldi, featsSURF, featsFAST;
	CImage				img;

	if (!img.loadFromFile(the_img_for_extract_feats ))
	{
		cerr << "Cannot load " << the_img_for_extract_feats  << endl;
		return;
	}
	cout << "Loaded test image: " << endl << the_img_for_extract_feats << endl;
	cout << "--------------------------------------------------------------------------" << endl << endl;

	CTicTac	tictac;

	fExt.options.patchSize = 0;

	cout << "Detect Harris features... [f_harris.txt]" << endl;
	tictac.Tic();
	fExt.options.featsType = featHarris;
	fExt.detectFeatures( img, featsHarris );
	cout << "Detected " << featsHarris.size() << " features in ";
	cout << format("  %.03fms",tictac.Tac()*1000) << endl << endl;
	featsHarris.saveToTextFile("f_harris.txt");
	wind1.setWindowTitle("Harris detected features");
	wind1.showImageAndPoints(img, featsHarris);

	cout << "Detect FAST features... [f_fast.txt]" << endl;
	tictac.Tic();
	fExt.options.featsType = featFAST;
	fExt.options.FASTOptions.threshold = 15; //150;
	fExt.options.FASTOptions.min_distance = 4;
	fExt.options.FASTOptions.use_KLT_response = true;
	fExt.detectFeatures( img, featsFAST, 0,  500 /* max num feats */  );
	cout << "Detected " << featsFAST.size() << " features in ";
	cout << format("  %.03fms",tictac.Tac()*1000) << endl << endl;
	featsFAST.saveToTextFile("f_fast.txt");
	wind5.setWindowTitle("FAST detected features");
	wind5.showImageAndPoints( img, featsFAST );

	cout << "Computing SIFT descriptors only ... [f_harris+sift.txt]" << endl;
	tictac.Tic();
	fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
	fExt.computeDescriptors( img, featsHarris, descSIFT );
	cout << format("  %.03fms",tictac.Tac()*1000) << endl << endl;
	featsHarris.saveToTextFile("f_harris+sift.txt");

	cout << "Extracting KLT features... [f_klt.txt]" << endl;
	tictac.Tic();
	fExt.options.featsType = featKLT;
	fExt.options.KLTOptions.threshold	= 0.05f;
	fExt.options.KLTOptions.radius		= 5;
	fExt.detectFeatures( img, featsKLT, 0, 10 );
	cout << "Detected " << featsKLT.size() << " features in ";
	cout << format("  %.03fms",tictac.Tac()*1000) << endl << endl;
	featsKLT.saveToTextFile("f_klt.txt");
	wind2.setWindowTitle("KLT detected features");
	wind2.showImageAndPoints( img, featsKLT );

	cout << "Extracting SIFT features... [f_sift_hess.txt]" << endl;
	tictac.Tic();
	fExt.options.featsType = featSIFT;
	fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;
	fExt.detectFeatures( img, featsSIFT_Hess );
	cout << "Detected " << featsSIFT_Hess.size() << " features in ";
	cout << format("  %.03fms",tictac.Tac()*1000) << endl << endl;
	featsSIFT_Hess.saveToTextFile("f_sift_hess.txt");
	wind3.setWindowTitle("SIFT Hess detected features");
	wind3.showImageAndPoints( img, featsSIFT_Hess );

	cout << "Extracting SURF features... [f_surf.txt]" << endl;
	tictac.Tic();
	fExt.options.featsType = featSURF;
	fExt.detectFeatures( img, featsSURF );
	cout << "Detected " << featsSURF.size() << " features in ";
	cout << format("  %.03fms",tictac.Tac()*1000) << endl << endl;
	featsSURF.saveToTextFile("f_surf.txt");
	wind4.setWindowTitle("SURF detected features");
	wind4.showImageAndPoints( img, featsSURF );

	cout << "Computing spin images descriptors only ... [f_harris+spinimgs.txt]" << endl;
	tictac.Tic();
	fExt.options.SpinImagesOptions.radius = 13;
	fExt.options.SpinImagesOptions.hist_size_distance  = 10;
	fExt.options.SpinImagesOptions.hist_size_intensity = 10;
	fExt.computeDescriptors( img, featsHarris, descSpinImages );
	cout << format("  %.03fms",tictac.Tac()*1000) << endl << endl;
	featsHarris.saveToTextFile("f_harris+spinimgs.txt");
	
	mrpt::system::pause();

	return;
}

// ------------------------------------------------------
//		TestMatchingComparative
// ------------------------------------------------------
void TestMatchingComparative()
{
    // Take two images
    string imgL = myDataDir + string("imL_p01.jpg");		// Left image
	string imgR = myDataDir + string("imR_p01.jpg");		// Right image

    CImage im1, im2;
    im1.loadFromFile( imgL );
    im2.loadFromFile( imgR );

    size_t imW = im1.getWidth();
    size_t imH = im1.getHeight();

    CFeatureExtraction fExt;
    fExt.options.featsType                  = featFAST;
    fExt.options.patchSize                  = 21;
    fExt.options.SIFTOptions.implementation = CFeatureExtraction::Hess;

    // Find FAST features
    CFeatureList list1, list2;
    fExt.detectFeatures( im1, list1, 150 );
    // Compute SIFT & SURF descriptors
    fExt.computeDescriptors( im1, list1, descSIFT );
    fExt.computeDescriptors( im1, list1, descSURF );

    fExt.detectFeatures( im2, list2, 150 );
    // Compute SIFT & SURF descriptors
    fExt.computeDescriptors( im2, list2, descSIFT );
    fExt.computeDescriptors( im2, list2, descSURF );

    CFeatureList::iterator it1, it2;
    for( it1 = list1.begin(); it1 != list1.end(); ++it1 )
        im1.cross( (*it1)->x, (*it1)->y, TColor::red, '+');
    for( it2 = list2.begin(); it2 != list2.end(); ++it2 )
        im2.cross( (*it2)->x, (*it2)->y, TColor::red, '+');

    CDisplayWindow win, win2;
    win.setPos(0,0);
    win2.setPos(0,imH*1.5);
    CImage joinimage, copyjoinimage, copyInfoImage;
    size_t imW2 = 1280;
    size_t imH2 = 150;

    CImage infoimage( imW2, imH2, CH_RGB );

    joinimage.joinImagesHorz( im1, im2 );
    infoimage.filledRectangle( 0, 0, imW2, imH2, TColor(150,150,150) );
    infoimage.textOut( 20, imH2-53, "SAD", TColor::blue );
    infoimage.textOut( 20, imH2-41, "NCC", TColor::blue );
    infoimage.textOut( 20, imH2-29, "SIFT", TColor::blue );
    infoimage.textOut( 20, imH2-17, "SURF", TColor::blue );
    for( it1 = list1.begin(); it1 != list1.end(); ++it1 )
    {
        copyInfoImage = infoimage;
        copyjoinimage = joinimage;
        copyjoinimage.line( (*it1)->x, 0, (*it1)->x, imH, TColor::green );            // Horiz
        copyjoinimage.line( (*it1)->x+imW, 0, (*it1)->x+imW, imH, TColor::green );    // Horiz
        copyjoinimage.line( 0, (*it1)->y, imW+imW, (*it1)->y, TColor::green );        // Epipolar
        copyjoinimage.drawCircle( (*it1)->x, (*it1)->y, 4, TColor::green, 2 );        // Keypoint

        copyInfoImage.update_patch( (*it1)->patch, 0, 0 );
        bool firstMatch = true;
        int cnt = 0;
        int px = 80;
        double minsad = 1.0, maxncc = 0.0;
        float minsiftd = 1.0f, minsurfd = 1.0f;
        int idxsad = 0, idxncc = 0, idxsiftd = 0, idxsurfd = 0;

        for( it2 = list2.begin(); it2 != list2.end(); ++it2 )
        {
            if( fabs((*it1)->y-(*it2)->y) <= 1.0 && (*it1)->x > (*it2)->x )
            {
                    // Compute matching with SAD and Correlation and SIFT/SURF?
                    // Use epipolar constraints
                    // Compute SAD
                    double sad = mrpt::vision::computeSAD( (*it1)->patch, (*it2)->patch );
                    if( sad < minsad )
                    {
                        minsad = sad;
                        idxsad = cnt;
                    }
                    // Compute Correlation
                    double ncc;
                    size_t u, v;
                    mrpt::vision::openCV_cross_correlation( (*it1)->patch, (*it2)->patch, u, v, ncc );
                    if( ncc > maxncc )
                    {
                        maxncc = ncc;
                        idxncc = cnt;
                    }

                    // Compute distance between descriptors SIFT
                    float siftd = (*it1)->descriptorSIFTDistanceTo( *(*it2) );
                    if( siftd < minsiftd )
                    {
                        minsiftd = siftd;
                        idxsiftd = cnt;
//.........这里部分代码省略.........

// ----
void TTwist2D::asString(std::string& s) const
{
	s = mrpt::format("[%f %f %f]", vx, vy, RAD2DEG(omega));
}

void CPointCloudFilterByDistance::filter(
	/** [in,out] The input pointcloud, which will be modified upon return after
	   filtering. */
	mrpt::maps::CPointsMap* pc,
	/** [in] The timestamp of the input pointcloud */
	const mrpt::system::TTimeStamp pc_timestamp,
	/** [in] If nullptr, the PC is assumed to be given in global coordinates.
	   Otherwise, it will be transformed from local coordinates to global using
	   this transformation. */
	const mrpt::poses::CPose3D& cur_pc_pose,
	/** [in,out] additional in/out parameters */
	TExtraFilterParams* params)
{
	using namespace mrpt::poses;
	using namespace mrpt::math;
	using mrpt::square;

	MRPT_START;
	ASSERT_(pc_timestamp != INVALID_TIMESTAMP);
	ASSERT_(pc != nullptr);

	CSimplePointsMap::Ptr original_pc =
		mrpt::make_aligned_shared<CSimplePointsMap>();
	original_pc->copyFrom(*pc);

	// 1) Filter:
	// ---------------------
	const size_t N = pc->size();
	std::vector<bool> deletion_mask;
	deletion_mask.assign(N, false);
	size_t del_count = 0;

	// get reference, previous PC:
	ASSERT_(options.previous_keyframes >= 1);
	bool can_do_filter = true;

	std::vector<FrameInfo*> prev_pc;  // (options.previous_keyframes, nullptr);
	{
		auto it = m_last_frames.rbegin();
		for (int i = 0;
			 i < options.previous_keyframes && it != m_last_frames.rend();
			 ++i, ++it)
		{
			prev_pc.push_back(&it->second);
		}
	}

	if (prev_pc.size() < static_cast<size_t>(options.previous_keyframes))
	{
		can_do_filter = false;
	}
	else
	{
		for (int i = 0; can_do_filter && i < options.previous_keyframes; ++i)
		{
			if (mrpt::system::timeDifference(
					m_last_frames.rbegin()->first, pc_timestamp) >
				options.too_old_seconds)
			{
				can_do_filter = false;  // A required keyframe is too old
				break;
			}
		}
	}

	if (can_do_filter)
	{
		// Reference poses of each PC:
		// Previous: prev_pc.pose
		mrpt::aligned_std_vector<CPose3D> rel_poses;
		for (int k = 0; k < options.previous_keyframes; ++k)
		{
			const CPose3D rel_pose = cur_pc_pose - prev_pc[k]->pose;
			rel_poses.push_back(rel_pose);
		}

		// The idea is that we can now find matches between pt{i} in time_{k},
		// composed with rel_pose
		// with the local points in time_{k-1}.

		std::vector<TPoint3D> pt_km1(options.previous_keyframes);

		for (size_t i = 0; i < N; i++)
		{
			// get i-th point in time=k:
			TPoint3Df ptf_k;
			pc->getPointFast(i, ptf_k.x, ptf_k.y, ptf_k.z);
			const TPoint3D pt_k = TPoint3D(ptf_k);

			// Point, referred to time=k-1 frame of reference
			for (int k = 0; k < options.previous_keyframes; ++k)
				rel_poses[k].composePoint(pt_k, pt_km1[k]);

			// Look for neighbors in "time=k"
			std::vector<TPoint3D> neig_k;
			std::vector<float> neig_sq_dist_k;
			pc->kdTreeNClosestPoint3D(
				pt_k, 2 /*num queries*/, neig_k, neig_sq_dist_k);

			// Look for neighbors in "time=k-i"
//.........这里部分代码省略.........

void CMetricMapBuilderRBPF::drawCurrentEstimationToImage(CCanvas* img)
{
	using mrpt::round;

	unsigned int i, M = mapPDF.particlesCount();
	std::deque<TPose3D> path;
	unsigned int imgHeight = 0;

	MRPT_START

	const mrpt::maps::CMultiMetricMap* currentMetricMapEstimation =
		mapPDF.getCurrentMostLikelyMetricMap();

	ASSERT_(currentMetricMapEstimation->m_gridMaps.size() > 0);

	// Find which is the most likely path index:
	unsigned int bestPath = 0;
	double bestPathLik = -1;
	for (i = 0; i < M; i++)
	{
		if (mapPDF.getW(i) > bestPathLik)
		{
			bestPathLik = mapPDF.getW(i);
			bestPath = i;
		}
	}

	// Compute the length of the paths:
	mapPDF.getPath(0, path);

	// Adapt the canvas size:
	bool alreadyCopiedImage = false;
	auto g = currentMetricMapEstimation->m_gridMaps[0];
	{
		auto* obj = dynamic_cast<CImage*>(img);
		if (obj) obj->resize(g->getSizeX(), g->getSizeY(), mrpt::img::CH_GRAY);
	}
	if (!alreadyCopiedImage)
	{
		CImage imgGrid;

		// grid map as bitmap:
		// ----------------------------------
		g->getAsImage(imgGrid);

		img->drawImage(0, 0, imgGrid);
		imgHeight = imgGrid.getHeight();
	}

	int x1 = 0, x2 = 0, y1 = 0, y2 = 0;
	float x_min = g->getXMin();
	float y_min = g->getYMin();
	float resolution = g->getResolution();

	// Paths hypothesis:
	// ----------------------------------
	/***/
	for (i = 0; i <= M; i++)
	{
		if (i != bestPath || i == M)
		{
			mapPDF.getPath(i == M ? bestPath : i, path);

			size_t nPoses = path.size();

			// First point: (0,0)
			x2 = round((path[0].x - x_min) / resolution);
			y2 = round((path[0].y - y_min) / resolution);

			// Draw path in the bitmap:
			for (size_t j = 0; j < nPoses; j++)
			{
				// For next segment
				x1 = x2;
				y1 = y2;

				// Coordinates -> pixels
				x2 = round((path[j].x - x_min) / resolution);
				y2 = round((path[j].y - y_min) / resolution);

				// Draw line:
				img->line(
					x1, round((imgHeight - 1) - y1), x2,
					round((imgHeight - 1) - y2),
					i == M ? TColor(0, 0, 0)
						   : TColor(0x50, 0x50, 0x50),  // Color, gray levels,
					i == M ? 3 : 1  // Line width
				);
			}
		}
	}

	MRPT_END
}

void TPose3D::fromString(const std::string& s)
{
	CMatrixDouble m;
	if (!m.fromMatlabStringFormat(s))
		THROW_EXCEPTION("Malformed expression in ::fromString");
	ASSERTMSG_(
		m.rows() == 1 && m.cols() == 6, "Wrong size of vector in ::fromString");
	x = m.get_unsafe(0, 0);
	y = m.get_unsafe(0, 1);
	z = m.get_unsafe(0, 2);
	yaw = DEG2RAD(m.get_unsafe(0, 3));
	pitch = DEG2RAD(m.get_unsafe(0, 4));
	roll = DEG2RAD(m.get_unsafe(0, 5));
}

/*---------------------------------------------------------------
					read_double
 ---------------------------------------------------------------*/
double CConfigFileBase::read_double(
	const std::string& section, const std::string& name, double defaultValue,
	bool failIfNotFound) const
{
	return atof(
		readString(section, name, format("%.16e", defaultValue), failIfNotFound)
			.c_str());
}

/*---------------------------------------------------------------
					read_float
 ---------------------------------------------------------------*/
float CConfigFileBase::read_float(
	const std::string& section, const std::string& name, float defaultValue,
	bool failIfNotFound) const
{
	return (float)atof(
		readString(section, name, format("%.10e", defaultValue), failIfNotFound)
			.c_str());
}

/*---------------------------------------------------------------
					read_int
 ---------------------------------------------------------------*/
int CConfigFileBase::read_int(
	const std::string& section, const std::string& name, int defaultValue,
	bool failIfNotFound) const
{
	return atoi(
		readString(section, name, format("%i", defaultValue), failIfNotFound)
			.c_str());
}

void CConfigFileBase::write(
	const std::string& section, const std::string& name, double value,
	const int name_padding_width, const int value_padding_width,
	const std::string& comment)
{
	writeString(
		section, name,
		format(
			((std::abs(value) > 1e-4 && std::abs(value) < 1e4) || value == .0)
				? "%f"
				: "%e",
			value),
		name_padding_width, value_padding_width, comment);
}