Working rig !
Frank Dellaert
parent
8bdf5326c0
commit
cbc45c9f4e
|
|
@ -1317,7 +1317,7 @@ class Similarity3 {
|
|||
double scale() const;
|
||||
};
|
||||
|
||||
template <T>
|
||||
template <T = {gtsam::PinholePoseCal3_S2}>
|
||||
class CameraSet {
|
||||
CameraSet();
|
||||
|
||||
|
|
|
|||
|
|
@ -758,10 +758,17 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
|
|||
}
|
||||
|
||||
// Vector of Cameras - used by the Python/MATLAB wrapper
|
||||
using CameraSetPinholePoseCal3Bundler = CameraSet<PinholePose<Cal3Bundler>>;
|
||||
using CameraSetPinholePoseCal3_S2 = CameraSet<PinholePose<Cal3_S2>>;
|
||||
using CameraSetPinholePoseCal3DS2 = CameraSet<PinholePose<Cal3DS2>>;
|
||||
using CameraSetPinholePoseCal3Fisheye = CameraSet<PinholePose<Cal3Fisheye>>;
|
||||
using CameraSetPinholePoseCal3Unified = CameraSet<PinholePose<Cal3Unified>>;
|
||||
|
||||
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
|
||||
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
|
||||
using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
|
||||
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
|
||||
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
|
||||
|
||||
using CameraSetSpherical = CameraSet<SphericalCamera>;
|
||||
} // \namespace gtsam
|
||||
|
|
|
|||
|
|
@ -46,6 +46,30 @@ namespace gtsam {
|
|||
* calibration (i.e., are from the same camera), use SmartProjectionPoseFactor
|
||||
* instead! If the calibration should be optimized, as well, use
|
||||
* SmartProjectionFactor instead!
|
||||
*
|
||||
* <b>Note on Template Parameter `CAMERA`:</b>
|
||||
* While this factor is templated on `CAMERA` to allow for generality (e.g.,
|
||||
* `SphericalCamera`), the current internal implementation for linearization
|
||||
* (specifically, methods like `createHessianFactor` involving Schur complement
|
||||
* calculations inherited or adapted from base classes) has limitations. It
|
||||
* implicitly assumes that the CAMERA only has a Pose3 unknown.
|
||||
*
|
||||
* Consequently:
|
||||
* - This factor works correctly with `CAMERA` types where this is the case,
|
||||
* such as `PinholePose<CALIBRATION>` or `SphericalCamera`.
|
||||
* - Using `CAMERA` types where `dimension != 6`, such as
|
||||
* `PinholeCamera<CALIBRATION>` (which has dimension `6 + CalDim`), will lead
|
||||
* to compilation errors due to matrix dimension mismatches.
|
||||
*
|
||||
* Therefore, for standard pinhole cameras within a fixed rig, use
|
||||
* `PinholePose<CALIBRATION>` as the `CAMERA` template parameter when defining
|
||||
* the `CameraSet` passed to this factor's constructor.
|
||||
*
|
||||
* TODO(dellaert): Refactor the internal linearization logic (e.g., in
|
||||
* `createHessianFactor`) to explicitly compute Jacobians with respect to the
|
||||
* 6-DoF body pose by correctly applying the chain rule, rather than relying on
|
||||
* `traits<CAMERA>::dimension` for downstream calculations.
|
||||
*
|
||||
* @ingroup slam
|
||||
*/
|
||||
template <class CAMERA>
|
||||
|
|
|
|||
|
|
@ -63,7 +63,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 9,
|
||||
"execution_count": 1,
|
||||
"metadata": {
|
||||
"id": "imports_code"
|
||||
},
|
||||
|
|
@ -81,6 +81,7 @@
|
|||
" SmartProjectionParams,\n",
|
||||
" SmartProjectionRigFactorPinholePoseCal3_S2,\n",
|
||||
" PinholePoseCal3_S2,\n",
|
||||
" CameraSetPinholePoseCal3_S2,\n",
|
||||
" Cal3_S2,\n",
|
||||
")\n",
|
||||
"from gtsam.symbol_shorthand import X # Key for Pose3 variable (Body Pose)"
|
||||
|
|
@ -108,20 +109,112 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"execution_count": 3,
|
||||
"metadata": {
|
||||
"id": "create_example_code"
|
||||
},
|
||||
"outputs": [
|
||||
{
|
||||
"ename": "TypeError",
|
||||
"evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2()\n 2. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2(sharedNoiseModel: gtsam.gtsam.noiseModel.Base, cameraRig: gtsam::CameraSet<gtsam::PinholePose<gtsam::Cal3_S2>>, params: gtsam.gtsam.SmartProjectionParams = linearizationMode: 0 degeneracyMode: 0 rankTolerance = 1 enableEPI = 0 landmarkDistanceThreshold = -1 dynamicOutlierRejectionThreshold = -1 useLOST = 0 noise model)\n\nInvoked with: unit (2) \n, [PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 0 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n, PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 -0.1 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n], linearizationMode: 0\n degeneracyMode: 1\nrankTolerance = 1\nenableEPI = 0\nlandmarkDistanceThreshold = -1\ndynamicOutlierRejectionThreshold = -1\nuseLOST = 0\nnoise model\n\n",
|
||||
"output_type": "error",
|
||||
"traceback": [
|
||||
"\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
|
||||
"\u001b[0;31mTypeError\u001b[0m Traceback (most recent call last)",
|
||||
"Cell \u001b[0;32mIn[10], line 19\u001b[0m\n\u001b[1;32m 15\u001b[0m smart_params \u001b[38;5;241m=\u001b[39m SmartProjectionParams(linMode\u001b[38;5;241m=\u001b[39mgtsam\u001b[38;5;241m.\u001b[39mLinearizationMode\u001b[38;5;241m.\u001b[39mHESSIAN,\n\u001b[1;32m 16\u001b[0m degMode\u001b[38;5;241m=\u001b[39mgtsam\u001b[38;5;241m.\u001b[39mDegeneracyMode\u001b[38;5;241m.\u001b[39mZERO_ON_DEGENERACY)\n\u001b[1;32m 18\u001b[0m \u001b[38;5;66;03m# Factor type includes the Camera type\u001b[39;00m\n\u001b[0;32m---> 19\u001b[0m smart_factor \u001b[38;5;241m=\u001b[39m \u001b[43mSmartProjectionRigFactorPinholePoseCal3_S2\u001b[49m\u001b[43m(\u001b[49m\u001b[43msmart_noise\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43mrig_cameras\u001b[49m\u001b[43m,\u001b[49m\u001b[43m \u001b[49m\u001b[43msmart_params\u001b[49m\u001b[43m)\u001b[49m\n\u001b[1;32m 21\u001b[0m \u001b[38;5;66;03m# 3. Add measurements\u001b[39;00m\n\u001b[1;32m 22\u001b[0m \u001b[38;5;66;03m# Observation from Body Pose X(0), Camera 0\u001b[39;00m\n\u001b[1;32m 23\u001b[0m smart_factor\u001b[38;5;241m.\u001b[39madd(Point2(\u001b[38;5;241m300\u001b[39m, \u001b[38;5;241m200\u001b[39m), X(\u001b[38;5;241m0\u001b[39m), \u001b[38;5;241m0\u001b[39m)\n",
|
||||
"\u001b[0;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2()\n 2. gtsam.gtsam.SmartProjectionRigFactorPinholePoseCal3_S2(sharedNoiseModel: gtsam.gtsam.noiseModel.Base, cameraRig: gtsam::CameraSet<gtsam::PinholePose<gtsam::Cal3_S2>>, params: gtsam.gtsam.SmartProjectionParams = linearizationMode: 0 degeneracyMode: 0 rankTolerance = 1 enableEPI = 0 landmarkDistanceThreshold = -1 dynamicOutlierRejectionThreshold = -1 useLOST = 0 noise model)\n\nInvoked with: unit (2) \n, [PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 0 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n, PinholePose.pose R: [\n\t1, 0, 0;\n\t0, 1, 0;\n\t-0, 0, 1\n]\nt: 0.1 -0.1 0\nPinholePose.calibration[\n\t500, 0, 320;\n\t0, 500, 240;\n\t0, 0, 1\n]\n], linearizationMode: 0\n degeneracyMode: 1\nrankTolerance = 1\nenableEPI = 0\nlandmarkDistanceThreshold = -1\ndynamicOutlierRejectionThreshold = -1\nuseLOST = 0\nnoise model\n\n"
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"Smart factor involves 2 measurements from 2 unique poses.\n",
|
||||
"SmartFactorRig: SmartProjectionRigFactor: \n",
|
||||
" -- Measurement nr 0\n",
|
||||
"key: x0\n",
|
||||
"cameraId: 0\n",
|
||||
"camera in rig:\n",
|
||||
".pose R: [\n",
|
||||
"\t1, 0, 0;\n",
|
||||
"\t0, 1, 0;\n",
|
||||
"\t-0, 0, 1\n",
|
||||
"]\n",
|
||||
"t: 0.1 0 0\n",
|
||||
"camera in rig:\n",
|
||||
".calibration[\n",
|
||||
"\t500, 0, 320;\n",
|
||||
"\t0, 500, 240;\n",
|
||||
"\t0, 0, 1\n",
|
||||
"]\n",
|
||||
"-- Measurement nr 1\n",
|
||||
"key: x0\n",
|
||||
"cameraId: 1\n",
|
||||
"camera in rig:\n",
|
||||
".pose R: [\n",
|
||||
"\t1, 0, 0;\n",
|
||||
"\t0, 1, 0;\n",
|
||||
"\t-0, 0, 1\n",
|
||||
"]\n",
|
||||
"t: 0.1 -0.1 0\n",
|
||||
"camera in rig:\n",
|
||||
".calibration[\n",
|
||||
"\t500, 0, 320;\n",
|
||||
"\t0, 500, 240;\n",
|
||||
"\t0, 0, 1\n",
|
||||
"]\n",
|
||||
"-- Measurement nr 2\n",
|
||||
"key: x1\n",
|
||||
"cameraId: 0\n",
|
||||
"camera in rig:\n",
|
||||
".pose R: [\n",
|
||||
"\t1, 0, 0;\n",
|
||||
"\t0, 1, 0;\n",
|
||||
"\t-0, 0, 1\n",
|
||||
"]\n",
|
||||
"t: 0.1 0 0\n",
|
||||
"camera in rig:\n",
|
||||
".calibration[\n",
|
||||
"\t500, 0, 320;\n",
|
||||
"\t0, 500, 240;\n",
|
||||
"\t0, 0, 1\n",
|
||||
"]\n",
|
||||
"-- Measurement nr 3\n",
|
||||
"key: x1\n",
|
||||
"cameraId: 1\n",
|
||||
"camera in rig:\n",
|
||||
".pose R: [\n",
|
||||
"\t1, 0, 0;\n",
|
||||
"\t0, 1, 0;\n",
|
||||
"\t-0, 0, 1\n",
|
||||
"]\n",
|
||||
"t: 0.1 -0.1 0\n",
|
||||
"camera in rig:\n",
|
||||
".calibration[\n",
|
||||
"\t500, 0, 320;\n",
|
||||
"\t0, 500, 240;\n",
|
||||
"\t0, 0, 1\n",
|
||||
"]\n",
|
||||
"SmartProjectionFactor\n",
|
||||
"linearizationMode: 0\n",
|
||||
"triangulationParameters:\n",
|
||||
"rankTolerance = 1\n",
|
||||
"enableEPI = 0\n",
|
||||
"landmarkDistanceThreshold = -1\n",
|
||||
"dynamicOutlierRejectionThreshold = -1\n",
|
||||
"useLOST = 0\n",
|
||||
"noise model\n",
|
||||
"\n",
|
||||
"result:\n",
|
||||
"no point, status = 1\n",
|
||||
"\n",
|
||||
"SmartFactorBase, z = \n",
|
||||
"measurement 0, px = \n",
|
||||
"300\n",
|
||||
"200\n",
|
||||
"noise model = unit (2) \n",
|
||||
"measurement 1, px = \n",
|
||||
"250\n",
|
||||
"201\n",
|
||||
"noise model = unit (2) \n",
|
||||
"measurement 2, px = \n",
|
||||
"310\n",
|
||||
"210\n",
|
||||
"noise model = unit (2) \n",
|
||||
"measurement 3, px = \n",
|
||||
"260\n",
|
||||
"211\n",
|
||||
"noise model = unit (2) \n",
|
||||
" keys = { x0 x1 }\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
|
|
@ -135,7 +228,9 @@
|
|||
"body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n",
|
||||
"cam1 = PinholePoseCal3_S2(body_P_cam1, K)\n",
|
||||
"\n",
|
||||
"rig_cameras = [cam0,cam1]\n",
|
||||
"rig_cameras = gtsam.CameraSetPinholePoseCal3_S2()\n",
|
||||
"rig_cameras.push_back(cam0)\n",
|
||||
"rig_cameras.push_back(cam1)\n",
|
||||
"\n",
|
||||
"# 2. Create the Factor\n",
|
||||
"noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
|
||||
|
|
@ -180,7 +275,7 @@
|
|||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"execution_count": 4,
|
||||
"metadata": {
|
||||
"id": "eval_example_code"
|
||||
},
|
||||
|
|
@ -190,9 +285,10 @@
|
|||
"output_type": "stream",
|
||||
"text": [
|
||||
"Triangulated point result:\n",
|
||||
"Valid triangulation with point [0.70307883 0.20615766 5.18676602]\n",
|
||||
"<gtsam.gtsam.TriangulationResult object at 0x117264ff0>\n",
|
||||
"\n",
|
||||
"Total reprojection error (0.5 * sum(err^2/sigma^2)): 181.1904\n"
|
||||
"Triangulation failed, error calculation depends on degeneracyMode.\n",
|
||||
"Error when degenerate: 0.0\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
|
|
@ -219,98 +315,6 @@
|
|||
" total_error = smart_factor.error(values)\n",
|
||||
" print(f\"Error when degenerate: {total_error}\")"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {
|
||||
"id": "linearize_header_md"
|
||||
},
|
||||
"source": [
|
||||
"## Linearization"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"metadata": {
|
||||
"id": "linearize_desc_md"
|
||||
},
|
||||
"source": [
|
||||
"Linearization (currently restricted to HESSIAN mode) produces a `RegularHessianFactor` connecting the unique body pose (`Pose3`) variables involved."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": null,
|
||||
"metadata": {
|
||||
"id": "linearize_example_code"
|
||||
},
|
||||
"outputs": [
|
||||
{
|
||||
"name": "stdout",
|
||||
"output_type": "stream",
|
||||
"text": [
|
||||
"\n",
|
||||
"Linearized Factor (HessianFactor structure):\n",
|
||||
"RegularHessianFactor(6): density=100% keys={ x0 x1 }\n",
|
||||
"Augmented information matrix: (dimensions: 6, 6) : \n",
|
||||
"{\n",
|
||||
"\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n",
|
||||
"\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n",
|
||||
"\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n",
|
||||
"\t[ -15.09, -101.6, -33.87, 181.2, -46.13, 41.54; ]\n",
|
||||
"\t[ 3.829, 25.77, 8.589, -46.13, 11.71, -10.54; ]\n",
|
||||
"\t[ -3.448, -23.21, -7.737, 41.54, -10.54, 9.497; ]\n",
|
||||
"\t[ 1.257, 8.427, 2.81, -1.257, -8.427, -2.81; ]\n",
|
||||
"\t[ 8.427, 56.73, 18.91, -8.427, -56.73, -18.91; ]\n",
|
||||
"\t[ 2.81, 18.91, 6.302, -2.81, -18.91, -6.302; ]\n",
|
||||
"\t[ -15.09, -101.6, -33.87, 15.09, 101.6, 33.87; ]\n",
|
||||
"\t[ 3.829, 25.77, 8.589, -3.829, -25.77, -8.589; ]\n",
|
||||
"\t[ -3.448, -23.21, -7.737, 3.448, 23.21, 7.737; ]\n",
|
||||
"\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n",
|
||||
"\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n",
|
||||
"\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n",
|
||||
"\t[ 15.09, 101.6, 33.87, -181.2, 46.13, -41.54; ]\n",
|
||||
"\t[ -3.829, -25.77, -8.589, 46.13, -11.71, 10.54; ]\n",
|
||||
"\t[ 3.448, 23.21, 7.737, -41.54, 10.54, -9.497; ]\n",
|
||||
"}\n",
|
||||
"Augmented Diagonal Block [0,0]:\n",
|
||||
"[ 1.257, 8.427, 2.81; ]\n",
|
||||
"[ 8.427, 56.73, 18.91; ]\n",
|
||||
"[ 2.81, 18.91, 6.302; ]\n",
|
||||
"\n",
|
||||
"Augmented Diagonal Block [1,1]:\n",
|
||||
"[ 1.257, 8.427, 2.81; ]\n",
|
||||
"[ 8.427, 56.73, 18.91; ]\n",
|
||||
"[ 2.81, 18.91, 6.302; ]\n",
|
||||
"\n",
|
||||
"Off-Diagonal Block [0,1]:\n",
|
||||
"[ -1.257, -8.427, -2.81; ]\n",
|
||||
"[ -8.427, -56.73, -18.91; ]\n",
|
||||
"[ -2.81, -18.91, -6.302; ]\n",
|
||||
"\n",
|
||||
"Error vector:\n",
|
||||
"[-15.087; -101.588; -33.8672; 181.19; -46.1294; 41.5391; 15.087; 101.588; 33.8672; -181.19; 46.1294; -41.5391]\n",
|
||||
"Constant error term: 181.19\n"
|
||||
]
|
||||
}
|
||||
],
|
||||
"source": [
|
||||
"graph = NonlinearFactorGraph()\n",
|
||||
"graph.add(smart_factor)\n",
|
||||
"\n",
|
||||
"# Linearize (HESSIAN mode)\n",
|
||||
"linear_factor = smart_factor.linearize(values)\n",
|
||||
"\n",
|
||||
"if linear_factor:\n",
|
||||
" print(\"\\nLinearized Factor (HessianFactor structure):\")\n",
|
||||
" hessian_factor = gtsam.RegularHessianFactorPose3.Downcast(linear_factor)\n",
|
||||
" if hessian_factor:\n",
|
||||
" hessian_factor.print()\n",
|
||||
" else:\n",
|
||||
" print(\"Linearized factor is not a HessianFactor\")\n",
|
||||
"else:\n",
|
||||
" print(\"Linearization failed (likely due to triangulation degeneracy)\")"
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {
|
||||
|
|
|
|||
|
|
@ -157,6 +157,7 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
|
|||
|
||||
#include <gtsam/slam/SmartFactorBase.h>
|
||||
|
||||
// Currently not wrapping SphericalCamera, since measurement type is not Point2 but Unit3
|
||||
template <
|
||||
CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2,
|
||||
gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye,
|
||||
|
|
@ -289,6 +290,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
|
|||
};
|
||||
|
||||
#include <gtsam/slam/SmartProjectionRigFactor.h>
|
||||
// Only for PinholePose cameras -> PinholeCamera is not supported
|
||||
template <CAMERA = {gtsam::PinholePoseCal3_S2, gtsam::PinholePoseCal3DS2,
|
||||
gtsam::PinholePoseCal3Bundler,
|
||||
gtsam::PinholePoseCal3Fisheye,
|
||||
|
|
|
|||
Loading…
Reference in New Issue