54
54
Vec3 projected = R * x + t;
55
55
projected /= projected(2);
57
residuals[0] = projected(0) - T(marker_.x);
58
residuals[1] = projected(1) - T(marker_.y);
57
residuals[0] = (projected(0) - T(marker_.x)) * marker_.weight;
58
residuals[1] = (projected(1) - T(marker_.y)) * marker_.weight;
101
101
ceres::Problem problem;
103
// Add residual blocks to the problem.
104
int num_residuals = 0;
103
105
for (int i = 0; i < markers.size(); ++i) {
104
106
const Marker &marker = markers[i];
105
const EuclideanCamera &camera =
106
*reconstruction->CameraForImage(marker.image);
108
problem.AddResidualBlock(
109
new ceres::AutoDiffCostFunction<
110
EuclideanIntersectCostFunctor,
111
2, /* num_residuals */
112
3>(new EuclideanIntersectCostFunctor(marker, camera)),
107
if (marker.weight != 0.0) {
108
const EuclideanCamera &camera =
109
*reconstruction->CameraForImage(marker.image);
111
problem.AddResidualBlock(
112
new ceres::AutoDiffCostFunction<
113
EuclideanIntersectCostFunctor,
114
2, /* num_residuals */
115
3>(new EuclideanIntersectCostFunctor(marker, camera)),
122
// TODO(sergey): Once we'll update Ceres to the next version
123
// we wouldn't need this check anymore -- Ceres will deal with
124
// zero-sized problems nicely.
125
LG << "Number of residuals: " << num_residuals;
126
if (!num_residuals) {
127
LG << "Skipping running minimizer with zero residuals";
129
// We still add 3D point for the track regardless it was
130
// optimized or not. If track is a constant zero it'll use
131
// algebraic intersection result as a 3D coordinate.
133
Vec3 point = X.head<3>();
134
reconstruction->InsertPoint(markers[0].track, point);
117
139
// Configure the solve.