@@ -21,16 +21,19 @@ class TriangulationSphericalKernel : public robustEstimation::IRansacKernel<robu
2121 /* *
2222 * @brief Constructor.
2323 * @param[in] observations The observations 2d points in pixels.
24+ * @param[in] weights The observations weights.
2425 * @param[in] poses the transformation for each observation.
2526 * @param[in] intrinsics the camera intrinsic for each observation.
2627 */
2728 TriangulationSphericalKernel (
2829 const std::vector<Vec2> & observations,
30+ const std::vector<double > & weights,
2931 const std::vector<Eigen::Matrix4d>& poses,
3032 std::vector<std::shared_ptr<camera::IntrinsicBase>> & intrinsics
3133 )
3234 : _poses(poses)
3335 , _intrinsics(intrinsics)
36+ , _weights(weights)
3437 {
3538 for (int id = 0 ; id < observations.size (); id++)
3639 {
@@ -134,10 +137,10 @@ class TriangulationSphericalKernel : public robustEstimation::IRansacKernel<robu
134137 }
135138
136139 /* *
137- * @brief Error for the i-th view
138- * @param[in] sample The index of the view for which the error is computed.
140+ * @brief Error for the i-th observation
141+ * @param[in] sample The index of the observation for which the error is computed.
139142 * @param[in] model The 3D point.
140- * @return The estimation error for the given view and 3D point.
143+ * @return The estimation error for the given observation and 3D point.
141144 */
142145 double error (std::size_t sample, const ModelT & model) const override
143146 {
@@ -147,9 +150,11 @@ class TriangulationSphericalKernel : public robustEstimation::IRansacKernel<robu
147150 X = X / X (3 );
148151 }
149152
153+ double w = _weights[sample];
154+
150155 Vec2 residual = _intrinsics[sample]->residual (_poses[sample], X, _observations[sample], false );
151156
152- return residual.norm ();
157+ return residual.norm () * w ;
153158 }
154159
155160 /* *
@@ -217,6 +222,7 @@ class TriangulationSphericalKernel : public robustEstimation::IRansacKernel<robu
217222private:
218223 std::vector<Vec3> _lifted;
219224 std::vector<Vec2> _observations;
225+ std::vector<double > _weights;
220226 const std::vector<Eigen::Matrix4d> _poses;
221227 const std::vector<std::shared_ptr<camera::IntrinsicBase>> _intrinsics;
222228 multiview::TriangulateNViewsSphericalSolver _solver;
0 commit comments