Example #1
0
void Run_KF_SLAM(CConfigFile& cfgFile, const std::string& rawlogFileName)
{
	// The EKF-SLAM class:
	// Traits for this KF implementation (2D or 3D)
	using traits_t = kfslam_traits<IMPL>;
	using ekfslam_t = typename traits_t::ekfslam_t;

	ekfslam_t mapping;

	// The rawlog file:
	// ----------------------------------------
	const unsigned int rawlog_offset =
		cfgFile.read_int("MappingApplication", "rawlog_offset", 0);

	const unsigned int SAVE_LOG_FREQUENCY =
		cfgFile.read_int("MappingApplication", "SAVE_LOG_FREQUENCY", 1);

	const bool SAVE_DA_LOG =
		cfgFile.read_bool("MappingApplication", "SAVE_DA_LOG", true);

	const bool SAVE_3D_SCENES =
		cfgFile.read_bool("MappingApplication", "SAVE_3D_SCENES", true);
	const bool SAVE_MAP_REPRESENTATIONS = cfgFile.read_bool(
		"MappingApplication", "SAVE_MAP_REPRESENTATIONS", true);
	bool SHOW_3D_LIVE =
		cfgFile.read_bool("MappingApplication", "SHOW_3D_LIVE", false);
	const bool CAMERA_3DSCENE_FOLLOWS_ROBOT = cfgFile.read_bool(
		"MappingApplication", "CAMERA_3DSCENE_FOLLOWS_ROBOT", false);

#if !MRPT_HAS_WXWIDGETS
	SHOW_3D_LIVE = false;
#endif

	string OUT_DIR = cfgFile.read_string(
		"MappingApplication", "logOutput_dir", "OUT_KF-SLAM");
	string ground_truth_file =
		cfgFile.read_string("MappingApplication", "ground_truth_file", "");
	string ground_truth_file_robot = cfgFile.read_string(
		"MappingApplication", "ground_truth_file_robot", "");

	string ground_truth_data_association = cfgFile.read_string(
		"MappingApplication", "ground_truth_data_association", "");

	cout << "RAWLOG FILE:" << endl << rawlogFileName << endl;
	ASSERT_FILE_EXISTS_(rawlogFileName);
	CFileGZInputStream rawlogFile(rawlogFileName);

	cout << "---------------------------------------------------" << endl
		 << endl;

	deleteFilesInDirectory(OUT_DIR);
	createDirectory(OUT_DIR);

	// Load the config options for mapping:
	// ----------------------------------------
	mapping.loadOptions(cfgFile);
	mapping.KF_options.dumpToConsole();
	mapping.options.dumpToConsole();

	// debug:
	// mapping.KF_options.use_analytic_observation_jacobian = true;
	// mapping.KF_options.use_analytic_transition_jacobian = true;
	// mapping.KF_options.debug_verify_analytic_jacobians = true;

	// Is there ground truth of the robot poses??
	CMatrixDouble GT_PATH(0, 0);
	if (ground_truth_file_robot.size() && fileExists(ground_truth_file_robot))
	{
		GT_PATH.loadFromTextFile(ground_truth_file_robot);
		ASSERT_(GT_PATH.rows() > 0 && GT_PATH.cols() == 6);
	}

	// Is there a ground truth file of the data association?
	std::map<double, std::vector<int>>
		GT_DA;  // Map: timestamp -> vector(index in observation -> real index)
	mrpt::containers::bimap<int, int> DA2GTDA_indices;  // Landmark indices
	// bimapping: SLAM DA <--->
	// GROUND TRUTH DA
	if (!ground_truth_data_association.empty() &&
		fileExists(ground_truth_data_association))
	{
		CMatrixDouble mGT_DA;
		mGT_DA.loadFromTextFile(ground_truth_data_association);
		ASSERT_ABOVEEQ_(mGT_DA.cols(), 3);
		// Convert the loaded matrix into a std::map in GT_DA:
		for (int i = 0; i < mGT_DA.rows(); i++)
		{
			std::vector<int>& v = GT_DA[mGT_DA(i, 0)];
			if (v.size() <= mGT_DA(i, 1)) v.resize(mGT_DA(i, 1) + 1);
			v[mGT_DA(i, 1)] = mGT_DA(i, 2);
		}
		cout << "Loaded " << GT_DA.size()
			 << " entries from DA ground truth file\n";
	}

	// Create output file for DA perf:
	std::ofstream out_da_performance_log;
	{
		const std::string f = std::string(
			OUT_DIR + std::string("/data_association_performance.log"));
		out_da_performance_log.open(f.c_str());
		ASSERTMSG_(
			out_da_performance_log.is_open(),
			std::string("Error writing to: ") + f);

		// Header:
		out_da_performance_log
			<< "%           TIMESTAMP                INDEX_IN_OBS    TruePos "
			   "FalsePos TrueNeg FalseNeg  NoGroundTruthSoIDontKnow \n"
			<< "%--------------------------------------------------------------"
			   "--------------------------------------------------\n";
	}

	if (SHOW_3D_LIVE)
	{
		win3d = mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow3D>(
			"KF-SLAM live view", 800, 500);

		win3d->addTextMessage(
			0.01, 0.96, "Red: Estimated path", TColorf(0.8f, 0.8f, 0.8f), 100,
			MRPT_GLUT_BITMAP_HELVETICA_10);
		win3d->addTextMessage(
			0.01, 0.93, "Black: Ground truth path", TColorf(0.8f, 0.8f, 0.8f),
			101, MRPT_GLUT_BITMAP_HELVETICA_10);
	}

	// Create DA-log output file:
	std::ofstream out_da_log;
	if (SAVE_DA_LOG)
	{
		const std::string f =
			std::string(OUT_DIR + std::string("/data_association.log"));
		out_da_log.open(f.c_str());
		ASSERTMSG_(out_da_log.is_open(), std::string("Error writing to: ") + f);

		// Header:
		out_da_log << "%           TIMESTAMP                INDEX_IN_OBS    ID "
					  "   RANGE(m)    YAW(rad)   PITCH(rad) \n"
				   << "%-------------------------------------------------------"
					  "-------------------------------------\n";
	}

	// The main loop:
	// ---------------------------------------
	CActionCollection::Ptr action;
	CSensoryFrame::Ptr observations;
	size_t rawlogEntry = 0, step = 0;

	vector<TPose3D> meanPath;  // The estimated path
	typename traits_t::posepdf_t robotPose;
	const bool is_pose_3d = robotPose.state_length != 3;

	std::vector<typename IMPL::landmark_point_t> LMs;
	std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs;
	CMatrixDouble fullCov;
	CVectorDouble fullState;
	CTicTac kftictac;

	auto rawlogArch = mrpt::serialization::archiveFrom(rawlogFile);

	for (;;)
	{
		if (os::kbhit())
		{
			char pushKey = os::getch();
			if (27 == pushKey) break;
		}

		// Load action/observation pair from the rawlog:
		// --------------------------------------------------
		if (!CRawlog::readActionObservationPair(
				rawlogArch, action, observations, rawlogEntry))
			break;  // file EOF

		if (rawlogEntry >= rawlog_offset)
		{
			// Process the action and observations:
			// --------------------------------------------
			kftictac.Tic();

			mapping.processActionObservation(action, observations);

			const double tim_kf_iter = kftictac.Tac();

			// Get current state:
			// -------------------------------
			mapping.getCurrentState(robotPose, LMs, LM_IDs, fullState, fullCov);
			cout << "Mean pose: " << endl << robotPose.mean << endl;
			cout << "# of landmarks in the map: " << LMs.size() << endl;

			// Get the mean robot pose as 3D:
			const CPose3D robotPoseMean3D = CPose3D(robotPose.mean);

			// Build the path:
			meanPath.push_back(robotPoseMean3D.asTPose());

			// Save mean pose:
			if (!(step % SAVE_LOG_FREQUENCY))
			{
				const auto p = robotPose.mean.asVectorVal();
				p.saveToTextFile(
					OUT_DIR +
					format("/robot_pose_%05u.txt", (unsigned int)step));
			}

			// Save full cov:
			if (!(step % SAVE_LOG_FREQUENCY))
			{
				fullCov.saveToTextFile(
					OUT_DIR + format("/full_cov_%05u.txt", (unsigned int)step));
			}

			// Generate Data Association log?
			if (SAVE_DA_LOG)
			{
				const typename ekfslam_t::TDataAssocInfo& da =
					mapping.getLastDataAssociation();

				const CObservationBearingRange::Ptr obs =
					observations
						->getObservationByClass<CObservationBearingRange>();
				if (obs)
				{
					const CObservationBearingRange* obsRB = obs.get();
					const double tim =
						mrpt::system::timestampToDouble(obsRB->timestamp);

					for (size_t i = 0; i < obsRB->sensedData.size(); i++)
					{
						auto it = da.results.associations.find(i);
						int assoc_ID_in_SLAM;
						if (it != da.results.associations.end())
							assoc_ID_in_SLAM = it->second;
						else
						{
							// It should be a newly created LM:
							auto itNewLM = da.newly_inserted_landmarks.find(i);
							if (itNewLM != da.newly_inserted_landmarks.end())
								assoc_ID_in_SLAM = itNewLM->second;
							else
								assoc_ID_in_SLAM = -1;
						}

						out_da_log << format(
							"%35.22f %8i %10i %10f %12f %12f\n", tim, (int)i,
							assoc_ID_in_SLAM,
							(double)obsRB->sensedData[i].range,
							(double)obsRB->sensedData[i].yaw,
							(double)obsRB->sensedData[i].pitch);
					}
				}
			}

			// Save report on DA performance:
			{
				const typename ekfslam_t::TDataAssocInfo& da =
					mapping.getLastDataAssociation();

				const CObservationBearingRange::Ptr obs =
					observations
						->getObservationByClass<CObservationBearingRange>();
				if (obs)
				{
					const CObservationBearingRange* obsRB = obs.get();
					const double tim =
						mrpt::system::timestampToDouble(obsRB->timestamp);

					auto itDA = GT_DA.find(tim);

					for (size_t i = 0; i < obsRB->sensedData.size(); i++)
					{
						bool is_FP = false, is_TP = false, is_FN = false,
							 is_TN = false;

						if (itDA != GT_DA.end())
						{
							const std::vector<int>& vDA = itDA->second;
							ASSERT_BELOW_(i, vDA.size());
							const int GT_ASSOC = vDA[i];

							auto it = da.results.associations.find(i);
							if (it != da.results.associations.end())
							{
								// This observation was assigned the already
								// existing LM in the map: "it->second"
								// TruePos -> If that LM index corresponds to
								// that in the GT (with index mapping):

								// mrpt::containers::bimap<int,int>
								// DA2GTDA_indices;
								// // Landmark indices bimapping: SLAM DA <--->
								// GROUND TRUTH DA
								if (DA2GTDA_indices.hasKey(it->second))
								{
									const int slam_asigned_LM_idx =
										DA2GTDA_indices.direct(it->second);
									if (slam_asigned_LM_idx == GT_ASSOC)
										is_TP = true;
									else
										is_FP = true;
								}
								else
								{
									// Is this case possible? Assigned to an
									// index not ever seen for the first time
									// with a GT....
									//  Just in case:
									is_FP = true;
								}
							}
							else
							{
								// No pairing, but should be a newly created LM:
								auto itNewLM =
									da.newly_inserted_landmarks.find(i);
								if (itNewLM !=
									da.newly_inserted_landmarks.end())
								{
									const int new_LM_in_SLAM = itNewLM->second;

									// Was this really a NEW LM not observed
									// before?
									if (DA2GTDA_indices.hasValue(GT_ASSOC))
									{
										// GT says this LM was already observed,
										// so it shouldn't appear here as new:
										is_FN = true;
									}
									else
									{
										// Really observed for the first time:
										is_TN = true;
										DA2GTDA_indices.insert(
											new_LM_in_SLAM, GT_ASSOC);
									}
								}
								else
								{
									// Not associated neither inserted:
									// Shouldn't really never arrive here.
								}
							}
						}

						// "%           TIMESTAMP                INDEX_IN_OBS
						// TruePos FalsePos TrueNeg FalseNeg
						// NoGroundTruthSoIDontKnow \n"
						out_da_performance_log << format(
							"%35.22f %13i %8i %8i %8i %8i %8i\n", tim, (int)i,
							(int)(is_TP ? 1 : 0), (int)(is_FP ? 1 : 0),
							(int)(is_TN ? 1 : 0), (int)(is_FN ? 1 : 0),
							(int)(!is_FP && !is_TP && !is_FN && !is_TN ? 1 : 0));
					}
				}
			}

			// Save map to file representations?
			if (SAVE_MAP_REPRESENTATIONS && !(step % SAVE_LOG_FREQUENCY))
			{
				mapping.saveMapAndPath2DRepresentationAsMATLABFile(
					OUT_DIR + format("/slam_state_%05u.m", (unsigned int)step));
			}

			// Save 3D view of the filter state:
			if (win3d || (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY)))
			{
				COpenGLScene::Ptr scene3D =
					mrpt::make_aligned_shared<COpenGLScene>();
				{
					opengl::CGridPlaneXY::Ptr grid =
						mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
							-1000, 1000, -1000, 1000, 0, 5);
					grid->setColor(0.4, 0.4, 0.4);
					scene3D->insert(grid);
				}

				// Robot path:
				{
					opengl::CSetOfLines::Ptr linesPath =
						mrpt::make_aligned_shared<opengl::CSetOfLines>();
					linesPath->setColor(1, 0, 0);

					TPose3D init_pose;
					if (!meanPath.empty())
						init_pose = CPose3D(meanPath[0]).asTPose();

					int path_decim = 0;
					for (auto& it : meanPath)
					{
						linesPath->appendLine(init_pose, it);
						init_pose = it;

						if (++path_decim > 10)
						{
							path_decim = 0;
							mrpt::opengl::CSetOfObjects::Ptr xyz =
								mrpt::opengl::stock_objects::CornerXYZSimple(
									0.3f, 2.0f);
							xyz->setPose(CPose3D(it));
							scene3D->insert(xyz);
						}
					}
					scene3D->insert(linesPath);

					// finally a big corner for the latest robot pose:
					{
						mrpt::opengl::CSetOfObjects::Ptr xyz =
							mrpt::opengl::stock_objects::CornerXYZSimple(
								1.0, 2.5);
						xyz->setPose(robotPoseMean3D);
						scene3D->insert(xyz);
					}

					// The camera pointing to the current robot pose:
					if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
					{
						win3d->setCameraPointingToPoint(
							robotPoseMean3D.x(), robotPoseMean3D.y(),
							robotPoseMean3D.z());
					}
				}

				// Do we have a ground truth?
				if (GT_PATH.cols() == 6 || GT_PATH.cols() == 3)
				{
					opengl::CSetOfLines::Ptr GT_path =
						mrpt::make_aligned_shared<opengl::CSetOfLines>();
					GT_path->setColor(0, 0, 0);
					size_t N =
						std::min((int)GT_PATH.rows(), (int)meanPath.size());

					if (GT_PATH.cols() == 6)
					{
						double gtx0 = 0, gty0 = 0, gtz0 = 0;
						for (size_t i = 0; i < N; i++)
						{
							const CPose3D p(
								GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2),
								GT_PATH(i, 3), GT_PATH(i, 4), GT_PATH(i, 5));

							GT_path->appendLine(
								gtx0, gty0, gtz0, p.x(), p.y(), p.z());
							gtx0 = p.x();
							gty0 = p.y();
							gtz0 = p.z();
						}
					}
					else if (GT_PATH.cols() == 3)
					{
						double gtx0 = 0, gty0 = 0;
						for (size_t i = 0; i < N; i++)
						{
							const CPose2D p(
								GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2));

							GT_path->appendLine(gtx0, gty0, 0, p.x(), p.y(), 0);
							gtx0 = p.x();
							gty0 = p.y();
						}
					}
					scene3D->insert(GT_path);
				}

				// Draw latest data association:
				{
					const typename ekfslam_t::TDataAssocInfo& da =
						mapping.getLastDataAssociation();

					mrpt::opengl::CSetOfLines::Ptr lins =
						mrpt::make_aligned_shared<mrpt::opengl::CSetOfLines>();
					lins->setLineWidth(1.2f);
					lins->setColor(1, 1, 1);
					for (auto it = da.results.associations.begin();
						 it != da.results.associations.end(); ++it)
					{
						const prediction_index_t idxPred = it->second;
						// This index must match the internal list of features
						// in the map:
						typename ekfslam_t::KFArray_FEAT featMean;
						mapping.getLandmarkMean(idxPred, featMean);

						TPoint3D featMean3D;
						traits_t::landmark_to_3d(featMean, featMean3D);

						// Line: robot -> landmark:
						lins->appendLine(
							robotPoseMean3D.x(), robotPoseMean3D.y(),
							robotPoseMean3D.z(), featMean3D.x, featMean3D.y,
							featMean3D.z);
					}
					scene3D->insert(lins);
				}

				// The current state of KF-SLAM:
				{
					opengl::CSetOfObjects::Ptr objs =
						mrpt::make_aligned_shared<opengl::CSetOfObjects>();
					mapping.getAs3DObject(objs);
					scene3D->insert(objs);
				}

				if (win3d)
				{
					mrpt::opengl::COpenGLScene::Ptr& scn =
						win3d->get3DSceneAndLock();
					scn = scene3D;

					// Update text messages:
					win3d->addTextMessage(
						0.02, 0.02,
						format(
							"Step %u - Landmarks in the map: %u",
							(unsigned int)step, (unsigned int)LMs.size()),
						TColorf(1, 1, 1), 0, MRPT_GLUT_BITMAP_HELVETICA_12);

					win3d->addTextMessage(
						0.02, 0.06,
						format(
							is_pose_3d
								? "Estimated pose: (x y z qr qx qy qz) = %s"
								: "Estimated pose: (x y yaw) = %s",
							robotPose.mean.asString().c_str()),
						TColorf(1, 1, 1), 1, MRPT_GLUT_BITMAP_HELVETICA_12);

					static vector<double> estHz_vals;
					const double curHz = 1.0 / std::max(1e-9, tim_kf_iter);
					estHz_vals.push_back(curHz);
					if (estHz_vals.size() > 50)
						estHz_vals.erase(estHz_vals.begin());
					const double meanHz = mrpt::math::mean(estHz_vals);

					win3d->addTextMessage(
						0.02, 0.10,
						format(
							"Iteration time: %7ss",
							mrpt::system::unitsFormat(tim_kf_iter).c_str()),
						TColorf(1, 1, 1), 2, MRPT_GLUT_BITMAP_HELVETICA_12);

					win3d->addTextMessage(
						0.02, 0.14,
						format(
							"Execution rate: %7sHz",
							mrpt::system::unitsFormat(meanHz).c_str()),
						TColorf(1, 1, 1), 3, MRPT_GLUT_BITMAP_HELVETICA_12);

					win3d->unlockAccess3DScene();
					win3d->repaint();
				}

				if (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY))
				{
					// Save to file:
					CFileGZOutputStream f(
						OUT_DIR +
						format("/kf_state_%05u.3Dscene", (unsigned int)step));
					mrpt::serialization::archiveFrom(f) << *scene3D;
				}
			}

			// Free rawlog items memory:
			// --------------------------------------------
			action.reset();
			observations.reset();

		}  // (rawlogEntry>=rawlog_offset)

		cout << format(
			"\nStep %u  - Rawlog entries processed: %i\n", (unsigned int)step,
			(unsigned int)rawlogEntry);

		step++;
	};  // end "while(1)"

	// Partitioning experiment: Only for 6D SLAM:
	traits_t::doPartitioningExperiment(mapping, fullCov, OUT_DIR);

	// Is there ground truth of landmarks positions??
	if (ground_truth_file.size() && fileExists(ground_truth_file))
	{
		CMatrixFloat GT(0, 0);
		try
		{
			GT.loadFromTextFile(ground_truth_file);
		}
		catch (const std::exception& e)
		{
			cerr << "Ignoring the following error loading ground truth file: "
				 << mrpt::exception_to_str(e) << endl;
		}

		if (GT.rows() > 0 && !LMs.empty())
		{
			// Each row has:
			//   [0] [1] [2]  [6]
			//    x   y   z    ID
			CVectorDouble ERRS(0);
			for (size_t i = 0; i < LMs.size(); i++)
			{
				// Find the entry in the GT for this mapped LM:
				bool found = false;
				for (int r = 0; r < GT.rows(); r++)
				{
					if (LM_IDs[i] == GT(r, 6))
					{
						const CPoint3D gtPt(GT(r, 0), GT(r, 1), GT(r, 2));
						ERRS.push_back(gtPt.distanceTo(
							CPoint3D(TPoint3D(LMs[i]))));  // All these
						// conversions
						// are to make it
						// work with
						// either
						// CPoint3D &
						// TPoint2D
						found = true;
						break;
					}
				}
				if (!found)
				{
					cerr << "Ground truth entry not found for landmark ID:"
						 << LM_IDs[i] << endl;
				}
			}

			printf("ERRORS VS. GROUND TRUTH:\n");
			printf("Mean Error: %f meters\n", math::mean(ERRS));
			printf("Minimum error: %f meters\n", math::minimum(ERRS));
			printf("Maximum error: %f meters\n", math::maximum(ERRS));
		}
	}  // end if GT

	cout << "********* KF-SLAM finished! **********" << endl;

	if (win3d)
	{
		cout << "\n Close the 3D window to quit the application.\n";
		win3d->waitForKey();
	}
}
Example #2
0
// ------------------------------------------------------
//                  TestGeometry3D
// ------------------------------------------------------
void TestLaser2Imgs()
{
	// Set your rawlog file name
	if (!mrpt::system::fileExists(RAWLOG_FILE))
		THROW_EXCEPTION_FMT(
			"Rawlog file does not exist: %s", RAWLOG_FILE.c_str())

	CActionCollection::Ptr action;
	CSensoryFrame::Ptr observations;
	size_t rawlogEntry = 0;
	// bool					end 			= false;
	CDisplayWindow wind;

	// Set relative path for externally-stored images in rawlogs:
	string rawlog_images_path = extractFileDirectory(RAWLOG_FILE);
	rawlog_images_path += "/Images";
	CImage::setImagesPathBase(rawlog_images_path);  // Set it.

	mrpt::io::CFileGZInputStream rawlogFile(RAWLOG_FILE);

	for (;;)
	{
		if (os::kbhit())
		{
			char c = os::getch();
			if (c == 27) break;
		}

		// Load action/observation pair from the rawlog:
		// --------------------------------------------------
		cout << "Reading act/oct pair from rawlog..." << endl;
		auto arch = mrpt::serialization::archiveFrom(rawlogFile);
		if (!CRawlog::readActionObservationPair(
				arch, action, observations, rawlogEntry))
			break;  // file EOF

		// CAMERA............
		// Get CObservationStereoImages
		CObservationStereoImages::Ptr sImgs;
		CObservationImage::Ptr Img;

		sImgs = observations->getObservationByClass<CObservationStereoImages>();
		if (!sImgs)
		{
			Img = observations->getObservationByClass<CObservationImage>();
			if (!Img) continue;
		}

		CPose3D cameraPose;  // Get Camera Pose (B) (CPose3D)
		CMatrixDouble33 K;  // Get Calibration matrix (K)

		sImgs ? sImgs->getSensorPose(cameraPose)
			  : Img->getSensorPose(cameraPose);
		K = sImgs ? sImgs->leftCamera.intrinsicParams
				  : Img->cameraParams.intrinsicParams;

		// LASER.............
		// Get CObservationRange2D
		CObservation2DRangeScan::Ptr laserScan =
			observations->getObservationByClass<CObservation2DRangeScan>();
		if (!laserScan) continue;

		// Get Laser Pose (A) (CPose3D)
		CPose3D laserPose;
		laserScan->getSensorPose(laserPose);

		if (abs(laserPose.yaw()) > DEG2RAD(90)) continue;  // Only front lasers

		// Get 3D Point relative to the Laser coordinate Frame (P1) (CPoint3D)
		CPoint3D point;
		CSimplePointsMap mapa;
		mapa.insertionOptions.minDistBetweenLaserPoints = 0;
		observations->insertObservationsInto(
			&mapa);  // <- The map contains the pose of the points (P1)

		// Get the points into the map
		vector<float> X, Y, Z;
		vector<float>::iterator itX, itY, itZ;
		mapa.getAllPoints(X, Y, Z);

		unsigned int imgW =
			sImgs ? sImgs->imageLeft.getWidth() : Img->image.getWidth();
		unsigned int imgH =
			sImgs ? sImgs->imageLeft.getHeight() : Img->image.getHeight();

		// unsigned int			idx = 0;
		vector<float> imgX, imgY;
		vector<float>::iterator itImgX, itImgY;
		imgX.resize(X.size());
		imgY.resize(Y.size());

		CImage image;
		image = sImgs ? sImgs->imageLeft : Img->image;

		// Get pixels in the image:
		// Pimg = (kx,ky,k)^T = K(I|0)*P2
		// Main loop
		for (itX = X.begin(), itY = Y.begin(), itZ = Z.begin(),
			itImgX = imgX.begin(), itImgY = imgY.begin();
			 itX != X.end(); itX++, itY++, itZ++, itImgX++, itImgY++)
		{
			// Coordinates Transformation
			CPoint3D pLaser(*itX, *itY, *itZ);
			CPoint3D pCamera(pLaser - cameraPose);

			if (pCamera.z() > 0)
			{
				*itImgX = -K(0, 0) * ((pCamera.x()) / (pCamera.z())) + K(0, 2);
				*itImgY = -K(1, 1) * ((pCamera.y()) / (pCamera.z())) + K(1, 2);

				if (*itImgX > 0 && *itImgX<imgW&& * itImgY> 0 && *itImgY < imgH)
					image.filledRectangle(
						*itImgX - 1, *itImgY - 1, *itImgX + 1, *itImgY + 1,
						TColor(255, 0, 0));
			}  // end if
		}  // end for

		action.reset();
		observations.reset();

		wind.showImage(image);

		std::this_thread::sleep_for(50ms);
	};  // end for

	mrpt::system::pause();
}