39 #include <cvsba/cvsba.h>
58 const std::map<int, Transform> & posesIn,
59 const std::multimap<int, Link> & links,
60 const std::map<
int, std::vector<CameraModel> > & models,
61 std::map<int, cv::Point3f> & points3DMap,
62 const std::map<
int, std::map<int, FeatureBA> > & wordReferences,
63 std::set<int> * outliers)
69 std::map<int, Transform> poses(posesIn.lower_bound(1), posesIn.end());
72 cvsba::Sba::Params
params ;
73 params.type = cvsba::Sba::MOTIONSTRUCTURE;
75 params.minError = this->
epsilon();
76 params.fixedIntrinsics = 5;
77 params.fixedDistortion = 5;
81 std::vector<cv::Mat> cameraMatrix(poses.size());
82 std::vector<cv::Mat>
R(poses.size());
83 std::vector<cv::Mat>
T(poses.size());
84 std::vector<cv::Mat> distCoeffs(poses.size());
85 std::map<int, int> frameIdToIndex;
87 for(std::map<int, Transform>::const_iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)
90 std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(
iter->first);
91 UASSERT(iterModel != models.end());
92 if(iterModel->second.size() != 1)
94 UERROR(
"Multi-camera BA not implemented for cvsba, only single camera.");
95 return std::map<int, Transform>();
97 UASSERT(iterModel->second[0].isValidForProjection());
99 frameIdToIndex.insert(std::make_pair(
iter->first, oi));
101 cameraMatrix[oi] = iterModel->second[0].K();
102 if(iterModel->second[0].D().cols != 5)
104 distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
105 UWARN(
"Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)",
iter->first);
109 distCoeffs[oi] = iterModel->second[0].D();
114 R[oi] = (cv::Mat_<double>(3,3) <<
115 (double)
t.r11(), (double)
t.r12(), (double)
t.r13(),
116 (double)
t.r21(), (double)
t.r22(), (double)
t.r23(),
117 (double)
t.r31(), (double)
t.r32(), (double)
t.r33());
118 T[oi] = (cv::Mat_<double>(1,3) << (double)
t.
x(), (double)
t.
y(), (double)
t.z());
121 UDEBUG(
"Pose %d = %s",
iter->first,
t.prettyPrint().c_str());
123 cameraMatrix.resize(oi);
126 distCoeffs.resize(oi);
128 UDEBUG(
"points=%d frames=%d", (
int)points3DMap.size(), (
int)poses.size());
129 std::vector<cv::Point3f> points(points3DMap.size());
130 std::vector<std::vector<cv::Point2f> > imagePoints(poses.size());
131 std::vector<std::vector<int> > visibility(poses.size());
132 for(
unsigned int i=0;
i<poses.size(); ++
i)
134 imagePoints[
i].resize(wordReferences.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
135 visibility[
i].resize(wordReferences.size(), 0);
138 for(std::map<int, cv::Point3f>::const_iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
140 points[
i] = kter->second;
142 std::map<int, std::map<int, FeatureBA> >::const_iterator
iter = wordReferences.find(kter->first);
143 if(
iter != wordReferences.end())
145 for(std::map<int, FeatureBA>::const_iterator jter=
iter->second.begin(); jter!=
iter->second.end(); ++jter)
147 if(frameIdToIndex.find(jter->first) != frameIdToIndex.end())
149 imagePoints[frameIdToIndex.at(jter->first)][
i] = cv::Point2f(jter->second.kpt.pt.x, jter->second.kpt.pt.y);
150 visibility[frameIdToIndex.at(jter->first)][
i] = 1;
160 sba.run( points, imagePoints, visibility, cameraMatrix,
R,
T, distCoeffs);
162 catch(cv::Exception &
e)
164 UERROR(
"Running SBA... error! %s",
e.what());
165 return std::map<int, Transform>();
170 std::map<int, Transform> newPoses = poses;
171 for(std::map<int, Transform>::iterator
iter=newPoses.begin();
iter!=newPoses.end(); ++
iter)
173 Transform t(
R[
i].at<double>(0,0),
R[
i].at<double>(0,1),
R[
i].at<double>(0,2),
T[
i].at<double>(0),
174 R[
i].at<double>(1,0),
R[
i].at<double>(1,1),
R[
i].at<double>(1,2),
T[
i].at<double>(1),
175 R[
i].at<double>(2,0),
R[
i].at<double>(2,1),
R[
i].at<double>(2,2),
T[
i].at<double>(2));
177 UDEBUG(
"New pose %d = %s",
iter->first,
t.prettyPrint().c_str());
181 t = (models.at(
iter->first)[0].localTransform() *
t).
inverse();
182 t =
iter->second.inverse() *
t;
183 iter->second *=
t.to3DoF();
195 for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
197 kter->second = points[
i++];
203 UERROR(
"RTAB-Map is not built with cvsba!");
204 return std::map<int, Transform>();