Re: RGBD SLAM and then RGB Localization

Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/RGBD-SLAM-and-then-RGB-Localization-tp489p500.html

Hi,

1) I think the best way to initialize the OodmetryMono local 3D map is to initialize the memory_ object with the database directly. The database doesn't save optimized poses of the map, so you should optimize them before adding the points to the local map. Here an example by modifying the lines 126-129 of OdometrMono to this:

customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
std::string myDatabasePath = "rtabmap.db";
if(!memory_->init(myDatabasePath, false, ParametersMap()))
{
	UERROR("Error initializing the memory for Mono Odometry.");
}
else
{
	// get the graph
	std::map<int, int> ids = memory_->getNeighborsId(memory_->getLastSignatureId(), 0, 0);
	std::map<int, Transform> poses;
	std::multimap<int, Link> links;
	memory_->getMetricConstraints(uKeysSet(ids), poses, links);

	//optimize the graph
	graph::TOROOptimizer optimizer;
	std::map<int, Transform> optimizedPoses = optimizer.optimize(poses.begin()->first, poses, links);

	// fill the local map
	for(std::map<int, Transform>::iterator posesIter=optimizedPoses.begin();
		posesIter!=optimizedPoses.end();
		++posesIter)
	{
		const Signature * s = memory_->getSignature(posesIter->first);
		if(s)
		{
			// Transform 3D points accordingly to pose and add them to local map
			const std::multimap<int, pcl::PointXYZ> & words3D = s->getWords3();
			for(std::multimap<int, pcl::PointXYZ>::const_iterator pointsIter=words3D.begin();
				pointsIter!=words3D.end();
				++pointsIter)
			{
				if(!uContains(localMap_, pointsIter->first))
				{
					localMap_.insert(std::make_pair(pointsIter->first, util3d::transformPoint(pointsIter->second, posesIter->second)));
				}
			}
		}
	}
}

2) The RGB images should be rectified. If they are already rectified, you only need to specify the focal lengths fx and fy (also cx and cy) for the sensor data sent to odometry. If you don't know the fx and fy values, you can calibrate your camera with rtabmap-calibration tool. Unlike CameraRGBD classes, which explicitly load calibration files, the Camera classes don't set fx,fy in CameraThread (see here). I plan to merge the Camera and CameraRGBD classes one day, but for now you can set the fx and fy values hard-coded. Example for line 133:

SensorData data(rgb, cv::Mat(), fx, fy, cx, cy, Transform(0,0,1,0,-1,0,0,0,0,-1,0,0), Transform(), 1, 1, ++_seq, UTimer::now());
this->post(new CameraEvent(data));

2a) Note that you could use the already calibrated RGB images from the Kinect for testing with OdometryMono (using CameraFreenect instead of CameraImages for example). There is an option to set CameraRGBD classes to send only RGB images.



cheers