diff --git a/.gitignore b/.gitignore index 0eb07fa..57ff58b 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ build *~ .idea CMakeLists.txt.user +*.swp diff --git a/include/steam.hpp b/include/steam.hpp index c326630..de57eea 100644 --- a/include/steam.hpp +++ b/include/steam.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -34,6 +35,9 @@ #include #include #include +#include +#include +#include // evaluator - block auto diff #include @@ -59,5 +63,6 @@ // trajectory #include +#include #endif // STEAM_ESTIMATION_LIBRARY_HPP diff --git a/include/steam/data/PointToPoint/points_simulated.txt b/include/steam/data/PointToPoint/points_simulated.txt new file mode 100644 index 0000000..12bf6e0 --- /dev/null +++ b/include/steam/data/PointToPoint/points_simulated.txt @@ -0,0 +1,400 @@ +-26.151,-4.4462,34.933,1 +-16.45,-74.15,-52.728,1 +30.849,69.365,34.645,1 +-0.16151,4.2019,131.2,1 +-113.16,-22.276,35.86,1 +4.4604,46.248,-45.118,1 +-11.489,6.6153,-2.7868,1 +74.286,140.76,-0.69856,1 +6.8027,-61.818,90.153,1 +32.477,129.5,81.223,1 +23.259,-9.6475,-114.41,1 +-32.788,24.984,-14.248,1 +-20.835,82.308,-59.545,1 +-53.601,0.42286,9.7717,1 +26.161,74.836,-2.1497,1 +-37.459,-72.008,73.002,1 +38.287,-29.968,-33.513,1 +-10.775,-5.9006,14.947,1 +38.304,-19.489,-23.726,1 +83.951,37.293,-49.289,1 +-26.201,-4.4463,34.933,1 +-16.55,-74.15,-52.728,1 +30.698,69.366,34.645,1 +-0.3614,4.2019,131.2,1 +-113.41,-22.279,35.86,1 +4.1588,46.248,-45.118,1 +-11.838,6.6146,-2.7868,1 +73.876,140.77,-0.69856,1 +6.36,-61.817,90.153,1 +31.962,129.5,81.223,1 +22.712,-9.6439,-114.41,1 +-33.39,24.978,-14.248,1 +-21.5,82.304,-59.545,1 +-54.298,0.40968,9.7717,1 +25.393,74.844,-2.1497,1 +-38.233,-72.02,73.002,1 +37.451,-29.954,-33.513,1 +-11.668,-5.9051,14.947,1 +37.368,-19.472,-23.726,1 +82.938,37.335,-49.289,1 +-27.193,-4.4609,34.933,1 +-17.499,-74.161,-52.728,1 +29.66,69.385,34.645,1 +-1.3573,4.2013,131.2,1 +-114.38,-22.365,35.86,1 +3.1298,46.251,-45.118,1 +-12.836,6.6042,-2.7868,1 +72.758,140.83,-0.69856,1 +5.4282,-61.812,90.153,1 +30.843,129.53,81.223,1 +21.732,-9.6203,-114.41,1 +-34.407,24.941,-14.248,1 +-22.583,82.279,-59.545,1 +-55.287,0.34435,9.7717,1 +24.312,74.875,-2.1497,1 +-39.126,-72.07,73.002,1 +36.505,-29.904,-33.513,1 +-12.646,-5.9217,14.947,1 +36.41,-19.418,-23.726,1 +81.896,37.459,-49.289,1 +-28.171,-4.5034,34.933,1 +-18.365,-74.189,-52.728,1 +28.562,69.434,34.645,1 +-2.3475,4.1987,131.2,1 +-115.33,-22.565,35.86,1 +2.0646,46.257,-45.118,1 +-13.829,6.5803,-2.7868,1 +71.509,140.97,-0.69856,1 +4.5683,-61.801,90.153,1 +29.604,129.59,81.223,1 +20.772,-9.5757,-114.41,1 +-35.438,24.869,-14.248,1 +-23.739,82.23,-59.545,1 +-56.265,0.22298,9.7717,1 +23.166,74.93,-2.1497,1 +-39.937,-72.159,73.002,1 +35.599,-29.818,-33.513,1 +-13.608,-5.9517,14.947,1 +35.482,-19.328,-23.726,1 +80.828,37.664,-49.289,1 +-29.134,-4.5749,34.933,1 +-19.146,-74.236,-52.728,1 +27.404,69.51,34.645,1 +-3.3318,4.1926,131.2,1 +-116.24,-22.882,35.86,1 +0.96303,46.263,-45.118,1 +-14.819,6.5413,-2.7868,1 +70.129,141.18,-0.69856,1 +3.78,-61.787,90.153,1 +28.245,129.68,81.223,1 +19.832,-9.5113,-114.41,1 +-36.484,24.76,-14.248,1 +-24.966,82.156,-59.545,1 +-57.234,0.044133,9.7717,1 +21.955,75.006,-2.1497,1 +-40.666,-72.289,73.002,1 +34.732,-29.697,-33.513,1 +-14.554,-5.9966,14.947,1 +34.583,-19.204,-23.726,1 +79.73,37.948,-49.289,1 +-30.082,-4.677,34.933,1 +-19.842,-74.303,-52.728,1 +26.187,69.611,34.645,1 +-4.3103,4.1816,131.2,1 +-117.11,-23.316,35.86,1 +-0.17479,46.267,-45.118,1 +-15.806,6.4858,-2.7868,1 +68.617,141.45,-0.69856,1 +3.0636,-61.769,90.153,1 +26.766,129.8,81.223,1 +18.911,-9.4287,-114.41,1 +-37.544,24.613,-14.248,1 +-26.265,82.055,-59.545,1 +-58.191,-0.19359,9.7717,1 +20.678,75.101,-2.1497,1 +-41.312,-72.461,73.002,1 +33.904,-29.543,-33.513,1 +-15.483,-6.0577,14.947,1 +33.713,-19.047,-23.726,1 +78.604,38.309,-49.289,1 +-31.015,-4.811,34.933,1 +-20.454,-74.391,-52.728,1 +24.909,69.734,34.645,1 +-5.2829,4.1642,131.2,1 +-117.95,-23.869,35.86,1 +-1.3489,46.269,-45.118,1 +-16.789,6.4124,-2.7868,1 +66.972,141.79,-0.69856,1 +2.4187,-61.75,90.153,1 +25.166,129.93,81.223,1 +18.009,-9.329,-114.41,1 +-38.618,24.425,-14.248,1 +-27.636,81.923,-59.545,1 +-59.138,-0.49155,9.7717,1 +19.336,75.214,-2.1497,1 +-41.874,-72.674,73.002,1 +33.115,-29.356,-33.513,1 +-16.396,-6.1363,14.947,1 +32.871,-18.859,-23.726,1 +77.447,38.746,-49.289,1 +-31.932,-4.9782,34.933,1 +-20.981,-74.499,-52.728,1 +23.571,69.879,34.645,1 +-6.2497,4.139,131.2,1 +-118.76,-24.542,35.86,1 +-2.5593,46.266,-45.118,1 +-17.768,6.3195,-2.7868,1 +65.193,142.19,-0.69856,1 +1.8455,-61.728,90.153,1 +23.446,130.09,81.223,1 +17.125,-9.2134,-114.41,1 +-39.706,24.196,-14.248,1 +-29.078,81.758,-59.545,1 +-60.072,-0.85109,9.7717,1 +17.928,75.34,-2.1497,1 +-42.353,-72.929,73.002,1 +32.364,-29.138,-33.513,1 +-17.293,-6.2336,14.947,1 +32.056,-18.639,-23.726,1 +76.258,39.256,-49.289,1 +-32.833,-5.1798,34.933,1 +-21.423,-74.629,-52.728,1 +22.173,70.042,34.645,1 +-7.2104,4.1046,131.2,1 +-119.52,-25.335,35.86,1 +-3.806,46.255,-45.118,1 +-18.742,6.2058,-2.7868,1 +63.279,142.64,-0.69856,1 +1.3439,-61.705,90.153,1 +21.605,130.26,81.223,1 +16.26,-9.0832,-114.41,1 +-40.806,23.923,-14.248,1 +-30.591,81.558,-59.545,1 +-60.992,-1.2735,9.7717,1 +16.454,75.479,-2.1497,1 +-42.746,-73.226,73.002,1 +31.651,-28.889,-33.513,1 +-18.172,-6.3508,14.947,1 +31.268,-18.389,-23.726,1 +75.036,39.837,-49.289,1 +-33.718,-5.417,34.933,1 +-21.78,-74.779,-52.728,1 +20.712,70.22,34.645,1 +-8.165,4.0597,131.2,1 +-120.24,-26.25,35.86,1 +-5.0888,46.235,-45.118,1 +-19.712,6.0698,-2.7868,1 +61.229,143.15,-0.69856,1 +0.91367,-61.681,90.153,1 +19.642,130.44,81.223,1 +15.413,-8.9393,-114.41,1 +-41.917,23.605,-14.248,1 +-32.173,81.319,-59.545,1 +-61.898,-1.76,9.7717,1 +14.913,75.626,-2.1497,1 +-43.054,-73.564,73.002,1 +30.974,-28.609,-33.513,1 +-19.035,-6.4891,14.947,1 +30.507,-18.11,-23.726,1 +73.78,40.488,-49.289,1 +-34.585,-5.691,34.933,1 +-22.05,-74.949,-52.728,1 +19.19,70.41,34.645,1 +-9.1133,4.0028,131.2,1 +-120.92,-27.287,35.86,1 +-6.4078,46.203,-45.118,1 +-20.677,5.9101,-2.7868,1 +59.042,143.71,-0.69856,1 +0.55482,-61.654,90.153,1 +17.557,130.63,81.223,1 +14.583,-8.7829,-114.41,1 +-43.04,23.239,-14.248,1 +-33.825,81.039,-59.545,1 +-62.787,-2.3118,9.7717,1 +13.305,75.778,-2.1497,1 +-43.276,-73.943,73.002,1 +30.334,-28.3,-33.513,1 +-19.879,-6.6496,14.947,1 +29.771,-17.802,-23.726,1 +72.486,41.205,-49.289,1 +-35.434,-6.0028,34.933,1 +-22.234,-75.139,-52.728,1 +17.606,70.611,34.645,1 +-10.055,3.9326,131.2,1 +-121.55,-28.446,35.86,1 +-7.7628,46.157,-45.118,1 +-21.636,5.7254,-2.7868,1 +56.715,144.3,-0.69856,1 +0.26723,-61.626,90.153,1 +15.349,130.82,81.223,1 +13.771,-8.615,-114.41,1 +-44.172,22.825,-14.248,1 +-35.544,80.713,-59.545,1 +-63.659,-2.93,9.7717,1 +11.629,75.933,-2.1497,1 +-43.409,-74.362,73.002,1 +29.728,-27.962,-33.513,1 +-20.706,-6.8332,14.947,1 +29.059,-17.468,-23.726,1 +71.154,41.986,-49.289,1 +-36.264,-6.3536,34.933,1 +-22.331,-75.348,-52.728,1 +15.959,70.817,34.645,1 +-10.99,3.8478,131.2,1 +-122.13,-29.728,35.86,1 +-9.1536,46.094,-45.118,1 +-22.589,5.5143,-2.7868,1 +54.247,144.94,-0.69856,1 +0.050762,-61.594,90.153,1 +13.019,131,81.223,1 +12.975,-8.4365,-114.41,1 +-45.312,22.359,-14.248,1 +-37.331,80.338,-59.545,1 +-64.511,-3.6157,9.7717,1 +9.8862,76.086,-2.1497,1 +-43.454,-74.82,73.002,1 +29.156,-27.595,-33.513,1 +-21.513,-7.0409,14.947,1 +28.371,-17.106,-23.726,1 +69.781,42.83,-49.289,1 +-37.074,-6.7441,34.933,1 +-22.341,-75.574,-52.728,1 +14.248,71.026,34.645,1 +-11.919,3.7471,131.2,1 +-122.66,-31.132,35.86,1 +-10.58,46.012,-45.118,1 +-23.535,5.2754,-2.7868,1 +51.636,145.6,-0.69856,1 +-0.094747,-61.559,90.153,1 +10.565,131.18,81.223,1 +12.195,-8.2483,-114.41,1 +-46.459,21.841,-14.248,1 +-39.184,79.911,-59.545,1 +-65.341,-4.3698,9.7717,1 +8.0748,76.235,-2.1497,1 +-43.409,-75.316,73.002,1 +28.617,-27.2,-33.513,1 +-22.301,-7.2736,14.947,1 +27.705,-16.718,-23.726,1 +68.364,43.732,-49.289,1 +-37.862,-7.1753,34.933,1 +-22.262,-75.816,-52.728,1 +12.474,71.234,34.645,1 +-12.84,3.6293,131.2,1 +-123.13,-32.659,35.86,1 +-12.041,45.907,-45.118,1 +-24.474,5.0074,-2.7868,1 +48.881,146.29,-0.69856,1 +-0.16949,-61.519,90.153,1 +7.9871,131.34,81.223,1 +11.431,-8.0513,-114.41,1 +-47.612,21.269,-14.248,1 +-41.102,79.427,-59.545,1 +-66.147,-5.1932,9.7717,1 +6.195,76.374,-2.1497,1 +-43.272,-75.847,73.002,1 +28.109,-26.776,-33.513,1 +-23.07,-7.5322,14.947,1 +27.06,-16.304,-23.726,1 +66.9,44.691,-49.289,1 +-38.627,-7.6481,34.933,1 +-22.094,-76.072,-52.728,1 +10.635,71.438,34.645,1 +-13.753,3.493,131.2,1 +-123.53,-34.309,35.86,1 +-13.537,45.777,-45.118,1 +-25.404,4.7091,-2.7868,1 +45.98,147,-0.69856,1 +-0.17369,-61.473,90.153,1 +5.2856,131.49,81.223,1 +10.681,-7.8463,-114.41,1 +-48.768,20.639,-14.248,1 +-43.082,78.882,-59.545,1 +-66.927,-6.0867,9.7717,1 +4.2465,76.5,-2.1497,1 +-43.042,-76.412,73.002,1 +27.632,-26.323,-33.513,1 +-23.817,-7.8174,14.947,1 +26.435,-15.865,-23.726,1 +65.388,45.702,-49.289,1 +-39.369,-8.163,34.933,1 +-21.838,-76.34,-52.728,1 +8.7318,71.632,34.645,1 +-14.658,3.337,131.2,1 +-123.87,-36.08,35.86,1 +-15.068,45.619,-45.118,1 +-26.325,4.3792,-2.7868,1 +42.931,147.72,-0.69856,1 +-0.10764,-61.42,90.153,1 +2.4604,131.6,81.223,1 +9.9461,-7.6342,-114.41,1 +-49.926,19.952,-14.248,1 +-45.123,78.272,-59.545,1 +-67.677,-7.051,9.7717,1 +2.2293,76.609,-2.1497,1 +-42.717,-77.009,73.002,1 +27.184,-25.843,-33.513,1 +-24.542,-8.1299,14.947,1 +25.829,-15.401,-23.726,1 +63.824,46.764,-49.289,1 +-40.084,-8.7209,34.933,1 +-21.491,-76.618,-52.728,1 +6.7633,71.814,34.645,1 +-15.555,3.1602,131.2,1 +-124.13,-37.973,35.86,1 +-16.632,45.43,-45.118,1 +-27.236,4.0165,-2.7868,1 +39.733,148.43,-0.69856,1 +0.028312,-61.357,90.153,1 +-0.48864,131.68,81.223,1 +9.2246,-7.4157,-114.41,1 +-51.083,19.204,-14.248,1 +-47.223,77.593,-59.545,1 +-68.395,-8.0866,9.7717,1 +0.14335,76.695,-2.1497,1 +-42.297,-77.634,73.002,1 +26.762,-25.334,-33.513,1 +-25.245,-8.4703,14.947,1 +25.24,-14.913,-23.726,1 +62.205,47.871,-49.289,1 +-40.773,-9.3221,34.933,1 +-21.054,-76.903,-52.728,1 +4.7294,71.977,34.645,1 +-16.442,2.9614,131.2,1 +-124.31,-39.985,35.86,1 +-18.229,45.206,-45.118,1 +-28.135,3.6199,-2.7868,1 +36.384,149.15,-0.69856,1 +0.23376,-61.284,90.153,1 +-3.561,131.72,81.223,1 +8.5162,-7.1915,-114.41,1 +-52.237,18.394,-14.248,1 +-49.379,76.839,-59.545,1 +-69.078,-9.1939,9.7717,1 +-2.0113,76.754,-2.1497,1 +-41.779,-78.286,73.002,1 +26.367,-24.797,-33.513,1 +-25.924,-8.8392,14.947,1 +24.666,-14.402,-23.726,1 +60.528,49.022,-49.289,1 +-41.432,-9.9671,34.933,1 +-20.526,-77.193,-52.728,1 +2.6301,72.119,34.645,1 +-17.32,2.7395,131.2,1 +-124.41,-42.115,35.86,1 +-19.857,44.945,-45.118,1 +-29.022,3.1882,-2.7868,1 +32.883,149.85,-0.69856,1 +0.50821,-61.197,90.153,1 +-6.7563,131.7,81.223,1 +7.82,-6.9624,-114.41,1 +-53.385,17.521,-14.248,1 +-51.589,76.006,-59.545,1 +-69.723,-10.373,9.7717,1 +-4.2344,76.782,-2.1497,1 +-41.162,-78.96,73.002,1 +25.996,-24.232,-33.513,1 +-26.579,-9.237,14.947,1 +24.106,-13.866,-23.726,1 +58.791,50.211,-49.289,1 diff --git a/include/steam/data/PointToPoint/poses_ic.txt b/include/steam/data/PointToPoint/poses_ic.txt new file mode 100644 index 0000000..7c796b8 --- /dev/null +++ b/include/steam/data/PointToPoint/poses_ic.txt @@ -0,0 +1,40 @@ +1,0,0,0,0,1,0,0,0,0,1,0 +1,-0.0005,0,-0.995,0.0005,1,0,-0.00024875,0,0,1,0 +1,-0.002,0,-1.98,0.002,1,0,-0.00198,0,0,1,0 +0.99999,-0.0045,0,-2.955,0.0045,0.99999,0,-0.0066487,0,0,1,0 +0.99997,-0.0079999,0,-3.92,0.0079999,0.99997,0,-0.01568,0,0,1,0 +0.99992,-0.0125,0,-4.8749,0.0125,0.99992,0,-0.030468,0,0,1,0 +0.99984,-0.017999,0,-5.8197,0.017999,0.99984,0,-0.052379,0,0,1,0 +0.9997,-0.024498,0,-6.7543,0.024498,0.9997,0,-0.082745,0,0,1,0 +0.99949,-0.031995,0,-7.6787,0.031995,0.99949,0,-0.12287,0,0,1,0 +0.99918,-0.040489,0,-8.5927,0.040489,0.99918,0,-0.17402,0,0,1,0 +0.99875,-0.049979,0,-9.496,0.049979,0.99875,0,-0.23745,0,0,1,0 +0.99817,-0.060463,0,-10.389,0.060463,0.99817,0,-0.31435,0,0,1,0 +0.99741,-0.071938,0,-11.27,0.071938,0.99741,0,-0.4059,0,0,1,0 +0.99643,-0.084399,0,-12.141,0.084399,0.99643,0,-0.51324,0,0,1,0 +0.9952,-0.097843,0,-12.999,0.097843,0.9952,0,-0.63747,0,0,1,0 +0.99368,-0.11226,0,-13.846,0.11226,0.99368,0,-0.77965,0,0,1,0 +0.99182,-0.12765,0,-14.68,0.12765,0.99182,0,-0.94079,0,0,1,0 +0.98958,-0.144,0,-15.501,0.144,0.98958,0,-1.1219,0,0,1,0 +0.98691,-0.16129,0,-16.308,0.16129,0.98691,0,-1.3239,0,0,1,0 +0.98375,-0.17952,0,-17.102,0.17952,0.98375,0,-1.5476,0,0,1,0 +-10,0,0,0,0,0,1,0,0,0,0,0.1 +-9.9,0,0,0,0,0.01,1,0,0,0,0,0.1 +-9.8,0,0,0,0,0.02,1,0,0,0,0,0.1 +-9.7,0,0,0,0,0.03,1,0,0,0,0,0.1 +-9.6,0,0,0,0,0.04,1,0,0,0,0,0.1 +-9.5,0,0,0,0,0.05,1,0,0,0,0,0.1 +-9.4,0,0,0,0,0.06,1,0,0,0,0,0.1 +-9.3,0,0,0,0,0.07,1,0,0,0,0,0.1 +-9.2,0,0,0,0,0.08,1,0,0,0,0,0.1 +-9.1,0,0,0,0,0.09,1,0,0,0,0,0.1 +-9,0,0,0,0,0.1,1,0,0,0,0,0.1 +-8.9,0,0,0,0,0.11,1,0,0,0,0,0.1 +-8.8,0,0,0,0,0.12,1,0,0,0,0,0.1 +-8.7,0,0,0,0,0.13,1,0,0,0,0,0.1 +-8.6,0,0,0,0,0.14,1,0,0,0,0,0.1 +-8.5,0,0,0,0,0.15,1,0,0,0,0,0.1 +-8.4,0,0,0,0,0.16,1,0,0,0,0,0.1 +-8.3,0,0,0,0,0.17,1,0,0,0,0,0.1 +-8.2,0,0,0,0,0.18,1,0,0,0,0,0.1 +-8.1,0,0,0,0,0.19,1,0,0,0,0,0.1 diff --git a/include/steam/data/PointToPoint_distorted/points_simulated.txt b/include/steam/data/PointToPoint_distorted/points_simulated.txt new file mode 100644 index 0000000..12bf6e0 --- /dev/null +++ b/include/steam/data/PointToPoint_distorted/points_simulated.txt @@ -0,0 +1,400 @@ +-26.151,-4.4462,34.933,1 +-16.45,-74.15,-52.728,1 +30.849,69.365,34.645,1 +-0.16151,4.2019,131.2,1 +-113.16,-22.276,35.86,1 +4.4604,46.248,-45.118,1 +-11.489,6.6153,-2.7868,1 +74.286,140.76,-0.69856,1 +6.8027,-61.818,90.153,1 +32.477,129.5,81.223,1 +23.259,-9.6475,-114.41,1 +-32.788,24.984,-14.248,1 +-20.835,82.308,-59.545,1 +-53.601,0.42286,9.7717,1 +26.161,74.836,-2.1497,1 +-37.459,-72.008,73.002,1 +38.287,-29.968,-33.513,1 +-10.775,-5.9006,14.947,1 +38.304,-19.489,-23.726,1 +83.951,37.293,-49.289,1 +-26.201,-4.4463,34.933,1 +-16.55,-74.15,-52.728,1 +30.698,69.366,34.645,1 +-0.3614,4.2019,131.2,1 +-113.41,-22.279,35.86,1 +4.1588,46.248,-45.118,1 +-11.838,6.6146,-2.7868,1 +73.876,140.77,-0.69856,1 +6.36,-61.817,90.153,1 +31.962,129.5,81.223,1 +22.712,-9.6439,-114.41,1 +-33.39,24.978,-14.248,1 +-21.5,82.304,-59.545,1 +-54.298,0.40968,9.7717,1 +25.393,74.844,-2.1497,1 +-38.233,-72.02,73.002,1 +37.451,-29.954,-33.513,1 +-11.668,-5.9051,14.947,1 +37.368,-19.472,-23.726,1 +82.938,37.335,-49.289,1 +-27.193,-4.4609,34.933,1 +-17.499,-74.161,-52.728,1 +29.66,69.385,34.645,1 +-1.3573,4.2013,131.2,1 +-114.38,-22.365,35.86,1 +3.1298,46.251,-45.118,1 +-12.836,6.6042,-2.7868,1 +72.758,140.83,-0.69856,1 +5.4282,-61.812,90.153,1 +30.843,129.53,81.223,1 +21.732,-9.6203,-114.41,1 +-34.407,24.941,-14.248,1 +-22.583,82.279,-59.545,1 +-55.287,0.34435,9.7717,1 +24.312,74.875,-2.1497,1 +-39.126,-72.07,73.002,1 +36.505,-29.904,-33.513,1 +-12.646,-5.9217,14.947,1 +36.41,-19.418,-23.726,1 +81.896,37.459,-49.289,1 +-28.171,-4.5034,34.933,1 +-18.365,-74.189,-52.728,1 +28.562,69.434,34.645,1 +-2.3475,4.1987,131.2,1 +-115.33,-22.565,35.86,1 +2.0646,46.257,-45.118,1 +-13.829,6.5803,-2.7868,1 +71.509,140.97,-0.69856,1 +4.5683,-61.801,90.153,1 +29.604,129.59,81.223,1 +20.772,-9.5757,-114.41,1 +-35.438,24.869,-14.248,1 +-23.739,82.23,-59.545,1 +-56.265,0.22298,9.7717,1 +23.166,74.93,-2.1497,1 +-39.937,-72.159,73.002,1 +35.599,-29.818,-33.513,1 +-13.608,-5.9517,14.947,1 +35.482,-19.328,-23.726,1 +80.828,37.664,-49.289,1 +-29.134,-4.5749,34.933,1 +-19.146,-74.236,-52.728,1 +27.404,69.51,34.645,1 +-3.3318,4.1926,131.2,1 +-116.24,-22.882,35.86,1 +0.96303,46.263,-45.118,1 +-14.819,6.5413,-2.7868,1 +70.129,141.18,-0.69856,1 +3.78,-61.787,90.153,1 +28.245,129.68,81.223,1 +19.832,-9.5113,-114.41,1 +-36.484,24.76,-14.248,1 +-24.966,82.156,-59.545,1 +-57.234,0.044133,9.7717,1 +21.955,75.006,-2.1497,1 +-40.666,-72.289,73.002,1 +34.732,-29.697,-33.513,1 +-14.554,-5.9966,14.947,1 +34.583,-19.204,-23.726,1 +79.73,37.948,-49.289,1 +-30.082,-4.677,34.933,1 +-19.842,-74.303,-52.728,1 +26.187,69.611,34.645,1 +-4.3103,4.1816,131.2,1 +-117.11,-23.316,35.86,1 +-0.17479,46.267,-45.118,1 +-15.806,6.4858,-2.7868,1 +68.617,141.45,-0.69856,1 +3.0636,-61.769,90.153,1 +26.766,129.8,81.223,1 +18.911,-9.4287,-114.41,1 +-37.544,24.613,-14.248,1 +-26.265,82.055,-59.545,1 +-58.191,-0.19359,9.7717,1 +20.678,75.101,-2.1497,1 +-41.312,-72.461,73.002,1 +33.904,-29.543,-33.513,1 +-15.483,-6.0577,14.947,1 +33.713,-19.047,-23.726,1 +78.604,38.309,-49.289,1 +-31.015,-4.811,34.933,1 +-20.454,-74.391,-52.728,1 +24.909,69.734,34.645,1 +-5.2829,4.1642,131.2,1 +-117.95,-23.869,35.86,1 +-1.3489,46.269,-45.118,1 +-16.789,6.4124,-2.7868,1 +66.972,141.79,-0.69856,1 +2.4187,-61.75,90.153,1 +25.166,129.93,81.223,1 +18.009,-9.329,-114.41,1 +-38.618,24.425,-14.248,1 +-27.636,81.923,-59.545,1 +-59.138,-0.49155,9.7717,1 +19.336,75.214,-2.1497,1 +-41.874,-72.674,73.002,1 +33.115,-29.356,-33.513,1 +-16.396,-6.1363,14.947,1 +32.871,-18.859,-23.726,1 +77.447,38.746,-49.289,1 +-31.932,-4.9782,34.933,1 +-20.981,-74.499,-52.728,1 +23.571,69.879,34.645,1 +-6.2497,4.139,131.2,1 +-118.76,-24.542,35.86,1 +-2.5593,46.266,-45.118,1 +-17.768,6.3195,-2.7868,1 +65.193,142.19,-0.69856,1 +1.8455,-61.728,90.153,1 +23.446,130.09,81.223,1 +17.125,-9.2134,-114.41,1 +-39.706,24.196,-14.248,1 +-29.078,81.758,-59.545,1 +-60.072,-0.85109,9.7717,1 +17.928,75.34,-2.1497,1 +-42.353,-72.929,73.002,1 +32.364,-29.138,-33.513,1 +-17.293,-6.2336,14.947,1 +32.056,-18.639,-23.726,1 +76.258,39.256,-49.289,1 +-32.833,-5.1798,34.933,1 +-21.423,-74.629,-52.728,1 +22.173,70.042,34.645,1 +-7.2104,4.1046,131.2,1 +-119.52,-25.335,35.86,1 +-3.806,46.255,-45.118,1 +-18.742,6.2058,-2.7868,1 +63.279,142.64,-0.69856,1 +1.3439,-61.705,90.153,1 +21.605,130.26,81.223,1 +16.26,-9.0832,-114.41,1 +-40.806,23.923,-14.248,1 +-30.591,81.558,-59.545,1 +-60.992,-1.2735,9.7717,1 +16.454,75.479,-2.1497,1 +-42.746,-73.226,73.002,1 +31.651,-28.889,-33.513,1 +-18.172,-6.3508,14.947,1 +31.268,-18.389,-23.726,1 +75.036,39.837,-49.289,1 +-33.718,-5.417,34.933,1 +-21.78,-74.779,-52.728,1 +20.712,70.22,34.645,1 +-8.165,4.0597,131.2,1 +-120.24,-26.25,35.86,1 +-5.0888,46.235,-45.118,1 +-19.712,6.0698,-2.7868,1 +61.229,143.15,-0.69856,1 +0.91367,-61.681,90.153,1 +19.642,130.44,81.223,1 +15.413,-8.9393,-114.41,1 +-41.917,23.605,-14.248,1 +-32.173,81.319,-59.545,1 +-61.898,-1.76,9.7717,1 +14.913,75.626,-2.1497,1 +-43.054,-73.564,73.002,1 +30.974,-28.609,-33.513,1 +-19.035,-6.4891,14.947,1 +30.507,-18.11,-23.726,1 +73.78,40.488,-49.289,1 +-34.585,-5.691,34.933,1 +-22.05,-74.949,-52.728,1 +19.19,70.41,34.645,1 +-9.1133,4.0028,131.2,1 +-120.92,-27.287,35.86,1 +-6.4078,46.203,-45.118,1 +-20.677,5.9101,-2.7868,1 +59.042,143.71,-0.69856,1 +0.55482,-61.654,90.153,1 +17.557,130.63,81.223,1 +14.583,-8.7829,-114.41,1 +-43.04,23.239,-14.248,1 +-33.825,81.039,-59.545,1 +-62.787,-2.3118,9.7717,1 +13.305,75.778,-2.1497,1 +-43.276,-73.943,73.002,1 +30.334,-28.3,-33.513,1 +-19.879,-6.6496,14.947,1 +29.771,-17.802,-23.726,1 +72.486,41.205,-49.289,1 +-35.434,-6.0028,34.933,1 +-22.234,-75.139,-52.728,1 +17.606,70.611,34.645,1 +-10.055,3.9326,131.2,1 +-121.55,-28.446,35.86,1 +-7.7628,46.157,-45.118,1 +-21.636,5.7254,-2.7868,1 +56.715,144.3,-0.69856,1 +0.26723,-61.626,90.153,1 +15.349,130.82,81.223,1 +13.771,-8.615,-114.41,1 +-44.172,22.825,-14.248,1 +-35.544,80.713,-59.545,1 +-63.659,-2.93,9.7717,1 +11.629,75.933,-2.1497,1 +-43.409,-74.362,73.002,1 +29.728,-27.962,-33.513,1 +-20.706,-6.8332,14.947,1 +29.059,-17.468,-23.726,1 +71.154,41.986,-49.289,1 +-36.264,-6.3536,34.933,1 +-22.331,-75.348,-52.728,1 +15.959,70.817,34.645,1 +-10.99,3.8478,131.2,1 +-122.13,-29.728,35.86,1 +-9.1536,46.094,-45.118,1 +-22.589,5.5143,-2.7868,1 +54.247,144.94,-0.69856,1 +0.050762,-61.594,90.153,1 +13.019,131,81.223,1 +12.975,-8.4365,-114.41,1 +-45.312,22.359,-14.248,1 +-37.331,80.338,-59.545,1 +-64.511,-3.6157,9.7717,1 +9.8862,76.086,-2.1497,1 +-43.454,-74.82,73.002,1 +29.156,-27.595,-33.513,1 +-21.513,-7.0409,14.947,1 +28.371,-17.106,-23.726,1 +69.781,42.83,-49.289,1 +-37.074,-6.7441,34.933,1 +-22.341,-75.574,-52.728,1 +14.248,71.026,34.645,1 +-11.919,3.7471,131.2,1 +-122.66,-31.132,35.86,1 +-10.58,46.012,-45.118,1 +-23.535,5.2754,-2.7868,1 +51.636,145.6,-0.69856,1 +-0.094747,-61.559,90.153,1 +10.565,131.18,81.223,1 +12.195,-8.2483,-114.41,1 +-46.459,21.841,-14.248,1 +-39.184,79.911,-59.545,1 +-65.341,-4.3698,9.7717,1 +8.0748,76.235,-2.1497,1 +-43.409,-75.316,73.002,1 +28.617,-27.2,-33.513,1 +-22.301,-7.2736,14.947,1 +27.705,-16.718,-23.726,1 +68.364,43.732,-49.289,1 +-37.862,-7.1753,34.933,1 +-22.262,-75.816,-52.728,1 +12.474,71.234,34.645,1 +-12.84,3.6293,131.2,1 +-123.13,-32.659,35.86,1 +-12.041,45.907,-45.118,1 +-24.474,5.0074,-2.7868,1 +48.881,146.29,-0.69856,1 +-0.16949,-61.519,90.153,1 +7.9871,131.34,81.223,1 +11.431,-8.0513,-114.41,1 +-47.612,21.269,-14.248,1 +-41.102,79.427,-59.545,1 +-66.147,-5.1932,9.7717,1 +6.195,76.374,-2.1497,1 +-43.272,-75.847,73.002,1 +28.109,-26.776,-33.513,1 +-23.07,-7.5322,14.947,1 +27.06,-16.304,-23.726,1 +66.9,44.691,-49.289,1 +-38.627,-7.6481,34.933,1 +-22.094,-76.072,-52.728,1 +10.635,71.438,34.645,1 +-13.753,3.493,131.2,1 +-123.53,-34.309,35.86,1 +-13.537,45.777,-45.118,1 +-25.404,4.7091,-2.7868,1 +45.98,147,-0.69856,1 +-0.17369,-61.473,90.153,1 +5.2856,131.49,81.223,1 +10.681,-7.8463,-114.41,1 +-48.768,20.639,-14.248,1 +-43.082,78.882,-59.545,1 +-66.927,-6.0867,9.7717,1 +4.2465,76.5,-2.1497,1 +-43.042,-76.412,73.002,1 +27.632,-26.323,-33.513,1 +-23.817,-7.8174,14.947,1 +26.435,-15.865,-23.726,1 +65.388,45.702,-49.289,1 +-39.369,-8.163,34.933,1 +-21.838,-76.34,-52.728,1 +8.7318,71.632,34.645,1 +-14.658,3.337,131.2,1 +-123.87,-36.08,35.86,1 +-15.068,45.619,-45.118,1 +-26.325,4.3792,-2.7868,1 +42.931,147.72,-0.69856,1 +-0.10764,-61.42,90.153,1 +2.4604,131.6,81.223,1 +9.9461,-7.6342,-114.41,1 +-49.926,19.952,-14.248,1 +-45.123,78.272,-59.545,1 +-67.677,-7.051,9.7717,1 +2.2293,76.609,-2.1497,1 +-42.717,-77.009,73.002,1 +27.184,-25.843,-33.513,1 +-24.542,-8.1299,14.947,1 +25.829,-15.401,-23.726,1 +63.824,46.764,-49.289,1 +-40.084,-8.7209,34.933,1 +-21.491,-76.618,-52.728,1 +6.7633,71.814,34.645,1 +-15.555,3.1602,131.2,1 +-124.13,-37.973,35.86,1 +-16.632,45.43,-45.118,1 +-27.236,4.0165,-2.7868,1 +39.733,148.43,-0.69856,1 +0.028312,-61.357,90.153,1 +-0.48864,131.68,81.223,1 +9.2246,-7.4157,-114.41,1 +-51.083,19.204,-14.248,1 +-47.223,77.593,-59.545,1 +-68.395,-8.0866,9.7717,1 +0.14335,76.695,-2.1497,1 +-42.297,-77.634,73.002,1 +26.762,-25.334,-33.513,1 +-25.245,-8.4703,14.947,1 +25.24,-14.913,-23.726,1 +62.205,47.871,-49.289,1 +-40.773,-9.3221,34.933,1 +-21.054,-76.903,-52.728,1 +4.7294,71.977,34.645,1 +-16.442,2.9614,131.2,1 +-124.31,-39.985,35.86,1 +-18.229,45.206,-45.118,1 +-28.135,3.6199,-2.7868,1 +36.384,149.15,-0.69856,1 +0.23376,-61.284,90.153,1 +-3.561,131.72,81.223,1 +8.5162,-7.1915,-114.41,1 +-52.237,18.394,-14.248,1 +-49.379,76.839,-59.545,1 +-69.078,-9.1939,9.7717,1 +-2.0113,76.754,-2.1497,1 +-41.779,-78.286,73.002,1 +26.367,-24.797,-33.513,1 +-25.924,-8.8392,14.947,1 +24.666,-14.402,-23.726,1 +60.528,49.022,-49.289,1 +-41.432,-9.9671,34.933,1 +-20.526,-77.193,-52.728,1 +2.6301,72.119,34.645,1 +-17.32,2.7395,131.2,1 +-124.41,-42.115,35.86,1 +-19.857,44.945,-45.118,1 +-29.022,3.1882,-2.7868,1 +32.883,149.85,-0.69856,1 +0.50821,-61.197,90.153,1 +-6.7563,131.7,81.223,1 +7.82,-6.9624,-114.41,1 +-53.385,17.521,-14.248,1 +-51.589,76.006,-59.545,1 +-69.723,-10.373,9.7717,1 +-4.2344,76.782,-2.1497,1 +-41.162,-78.96,73.002,1 +25.996,-24.232,-33.513,1 +-26.579,-9.237,14.947,1 +24.106,-13.866,-23.726,1 +58.791,50.211,-49.289,1 diff --git a/include/steam/data/PointToPoint_distorted/poses_ic.txt b/include/steam/data/PointToPoint_distorted/poses_ic.txt new file mode 100644 index 0000000..7c796b8 --- /dev/null +++ b/include/steam/data/PointToPoint_distorted/poses_ic.txt @@ -0,0 +1,40 @@ +1,0,0,0,0,1,0,0,0,0,1,0 +1,-0.0005,0,-0.995,0.0005,1,0,-0.00024875,0,0,1,0 +1,-0.002,0,-1.98,0.002,1,0,-0.00198,0,0,1,0 +0.99999,-0.0045,0,-2.955,0.0045,0.99999,0,-0.0066487,0,0,1,0 +0.99997,-0.0079999,0,-3.92,0.0079999,0.99997,0,-0.01568,0,0,1,0 +0.99992,-0.0125,0,-4.8749,0.0125,0.99992,0,-0.030468,0,0,1,0 +0.99984,-0.017999,0,-5.8197,0.017999,0.99984,0,-0.052379,0,0,1,0 +0.9997,-0.024498,0,-6.7543,0.024498,0.9997,0,-0.082745,0,0,1,0 +0.99949,-0.031995,0,-7.6787,0.031995,0.99949,0,-0.12287,0,0,1,0 +0.99918,-0.040489,0,-8.5927,0.040489,0.99918,0,-0.17402,0,0,1,0 +0.99875,-0.049979,0,-9.496,0.049979,0.99875,0,-0.23745,0,0,1,0 +0.99817,-0.060463,0,-10.389,0.060463,0.99817,0,-0.31435,0,0,1,0 +0.99741,-0.071938,0,-11.27,0.071938,0.99741,0,-0.4059,0,0,1,0 +0.99643,-0.084399,0,-12.141,0.084399,0.99643,0,-0.51324,0,0,1,0 +0.9952,-0.097843,0,-12.999,0.097843,0.9952,0,-0.63747,0,0,1,0 +0.99368,-0.11226,0,-13.846,0.11226,0.99368,0,-0.77965,0,0,1,0 +0.99182,-0.12765,0,-14.68,0.12765,0.99182,0,-0.94079,0,0,1,0 +0.98958,-0.144,0,-15.501,0.144,0.98958,0,-1.1219,0,0,1,0 +0.98691,-0.16129,0,-16.308,0.16129,0.98691,0,-1.3239,0,0,1,0 +0.98375,-0.17952,0,-17.102,0.17952,0.98375,0,-1.5476,0,0,1,0 +-10,0,0,0,0,0,1,0,0,0,0,0.1 +-9.9,0,0,0,0,0.01,1,0,0,0,0,0.1 +-9.8,0,0,0,0,0.02,1,0,0,0,0,0.1 +-9.7,0,0,0,0,0.03,1,0,0,0,0,0.1 +-9.6,0,0,0,0,0.04,1,0,0,0,0,0.1 +-9.5,0,0,0,0,0.05,1,0,0,0,0,0.1 +-9.4,0,0,0,0,0.06,1,0,0,0,0,0.1 +-9.3,0,0,0,0,0.07,1,0,0,0,0,0.1 +-9.2,0,0,0,0,0.08,1,0,0,0,0,0.1 +-9.1,0,0,0,0,0.09,1,0,0,0,0,0.1 +-9,0,0,0,0,0.1,1,0,0,0,0,0.1 +-8.9,0,0,0,0,0.11,1,0,0,0,0,0.1 +-8.8,0,0,0,0,0.12,1,0,0,0,0,0.1 +-8.7,0,0,0,0,0.13,1,0,0,0,0,0.1 +-8.6,0,0,0,0,0.14,1,0,0,0,0,0.1 +-8.5,0,0,0,0,0.15,1,0,0,0,0,0.1 +-8.4,0,0,0,0,0.16,1,0,0,0,0,0.1 +-8.3,0,0,0,0,0.17,1,0,0,0,0,0.1 +-8.2,0,0,0,0,0.18,1,0,0,0,0,0.1 +-8.1,0,0,0,0,0.19,1,0,0,0,0,0.1 diff --git a/include/steam/data/points_simulated.txt b/include/steam/data/points_simulated.txt new file mode 100644 index 0000000..e3ead1f --- /dev/null +++ b/include/steam/data/points_simulated.txt @@ -0,0 +1,400 @@ +-26.151,-4.4462,34.933,1 +-16.45,-74.15,-52.728,1 +30.849,69.365,34.645,1 +-0.16151,4.2019,131.2,1 +-113.16,-22.276,35.86,1 +4.4604,46.248,-45.118,1 +-11.489,6.6153,-2.7868,1 +74.286,140.76,-0.69856,1 +6.8027,-61.818,90.153,1 +32.477,129.5,81.223,1 +23.259,-9.6475,-114.41,1 +-32.788,24.984,-14.248,1 +-20.835,82.308,-59.545,1 +-53.601,0.42286,9.7717,1 +26.161,74.836,-2.1497,1 +-37.459,-72.008,73.002,1 +38.287,-29.968,-33.513,1 +-10.775,-5.9006,14.947,1 +38.304,-19.489,-23.726,1 +83.951,37.293,-49.289,1 +-27.129,-4.4862,34.933,1 +-17.324,-74.176,-52.728,1 +29.76,69.411,34.645,1 +-1.1528,4.2009,131.2,1 +-114.11,-22.446,35.86,1 +3.4061,46.254,-45.118,1 +-12.484,6.5973,-2.7868,1 +73.09,140.87,-0.69856,1 +5.9105,-61.809,90.153,1 +31.298,129.55,81.223,1 +22.288,-9.6133,-114.41,1 +-33.81,24.934,-14.248,1 +-21.943,82.276,-59.545,1 +-54.586,0.34172,9.7717,1 +25.064,74.875,-2.1497,1 +-38.336,-72.065,73.002,1 +37.347,-29.911,-33.513,1 +-11.751,-5.9175,14.947,1 +37.349,-19.432,-23.726,1 +82.91,37.418,-49.289,1 +-28.064,-4.6089,34.933,1 +-17.945,-74.253,-52.728,1 +28.492,69.543,34.645,1 +-2.1267,4.195,131.2,1 +-114.96,-22.96,35.86,1 +2.2429,46.268,-45.118,1 +-13.468,6.5404,-2.7868,1 +71.5,141.2,-0.69856,1 +5.2335,-61.782,90.153,1 +29.76,129.68,81.223,1 +21.376,-9.5135,-114.41,1 +-34.877,24.781,-14.248,1 +-23.268,82.176,-59.545,1 +-55.542,0.095427,9.7717,1 +23.772,74.986,-2.1497,1 +-38.966,-72.237,73.002,1 +36.526,-29.744,-33.513,1 +-12.679,-5.971,14.947,1 +36.481,-19.265,-23.726,1 +81.786,37.79,-49.289,1 +-28.953,-4.8182,34.933,1 +-18.313,-74.385,-52.728,1 +27.045,69.756,34.645,1 +-3.0831,4.18,131.2,1 +-115.71,-23.821,35.86,1 +0.9708,46.285,-45.118,1 +-14.442,6.4402,-2.7868,1 +69.514,141.73,-0.69856,1 +4.7717,-61.74,90.153,1 +27.861,129.9,81.223,1 +20.522,-9.3519,-114.41,1 +-35.987,24.52,-14.248,1 +-24.809,82,-59.545,1 +-56.466,-0.32011,9.7717,1 +22.284,75.163,-2.1497,1 +-39.348,-72.526,73.002,1 +35.823,-29.468,-33.513,1 +-13.559,-6.0649,14.947,1 +35.699,-18.989,-23.726,1 +80.575,38.404,-49.289,1 +-29.796,-5.1176,34.933,1 +-18.426,-74.569,-52.728,1 +25.416,70.041,34.645,1 +-4.0219,4.1517,131.2,1 +-116.35,-25.03,35.86,1 +-0.41032,46.297,-45.118,1 +-15.404,6.2926,-2.7868,1 +67.127,142.46,-0.69856,1 +4.5246,-61.682,90.153,1 +25.601,130.19,81.223,1 +19.724,-9.1316,-114.41,1 +-37.137,24.145,-14.248,1 +-26.564,81.74,-59.545,1 +-57.355,-0.90867,9.7717,1 +20.598,75.397,-2.1497,1 +-39.48,-72.931,73.002,1 +35.235,-29.086,-33.513,1 +-14.39,-6.2026,14.947,1 +35.001,-18.609,-23.726,1 +79.272,39.252,-49.289,1 +-30.59,-5.5102,34.933,1 +-18.283,-74.802,-52.728,1 +23.603,70.387,34.645,1 +-4.9428,4.1062,131.2,1 +-116.87,-26.589,35.86,1 +-1.9005,46.296,-45.118,1 +-16.353,6.0932,-2.7868,1 +64.333,143.36,-0.69856,1 +4.4917,-61.606,90.153,1 +22.976,130.54,81.223,1 +18.98,-8.8554,-114.41,1 +-38.325,23.651,-14.248,1 +-28.53,81.383,-59.545,1 +-58.203,-1.6737,9.7717,1 +18.713,75.678,-2.1497,1 +-39.357,-73.448,73.002,1 +34.759,-28.598,-33.513,1 +-15.17,-6.3871,14.947,1 +34.384,-18.126,-23.726,1 +77.87,40.328,-49.289,1 +-31.33,-5.9986,34.933,1 +-17.881,-75.077,-52.728,1 +21.603,70.782,34.645,1 +-5.8454,4.0397,131.2,1 +-117.25,-28.498,35.86,1 +-3.4996,46.274,-45.118,1 +-17.286,5.8382,-2.7868,1 +61.123,144.42,-0.69856,1 +4.672,-61.508,90.153,1 +19.983,130.91,81.223,1 +18.288,-8.5254,-114.41,1 +-39.546,23.031,-14.248,1 +-30.704,80.916,-59.545,1 +-59.003,-2.6182,9.7717,1 +16.627,75.992,-2.1497,1 +-38.975,-74.072,73.002,1 +34.391,-28.005,-33.513,1 +-15.898,-6.621,14.947,1 +33.843,-17.54,-23.726,1 +76.359,41.622,-49.289,1 +-32.013,-6.5847,34.933,1 +-17.22,-75.388,-52.728,1 +19.413,71.213,34.645,1 +-6.729,3.9485,131.2,1 +-117.48,-30.756,35.86,1 +-5.2071,46.221,-45.118,1 +-18.203,5.5236,-2.7868,1 +57.49,145.61,-0.69856,1 +5.0646,-61.382,90.153,1 +16.621,131.3,81.223,1 +17.645,-8.1436,-114.41,1 +-40.793,22.279,-14.248,1 +-33.082,80.326,-59.545,1 +-59.746,-3.7445,9.7717,1 +14.336,76.325,-2.1497,1 +-38.329,-74.794,73.002,1 +34.125,-27.306,-33.513,1 +-16.572,-6.9061,14.947,1 +33.373,-16.854,-23.726,1 +74.727,43.127,-49.289,1 +-32.634,-7.2701,34.933,1 +-16.296,-75.723,-52.728,1 +17.029,71.665,34.645,1 +-7.5927,3.8294,131.2,1 +-117.53,-33.358,35.86,1 +-7.0223,46.125,-45.118,1 +-19.099,5.1459,-2.7868,1 +53.423,146.9,-0.69856,1 +5.6677,-61.219,90.153,1 +12.886,131.68,81.223,1 +17.047,-7.7113,-114.41,1 +-42.061,21.389,-14.248,1 +-35.658,79.595,-59.545,1 +-60.423,-5.0545,9.7717,1 +11.839,76.662,-2.1497,1 +-37.414,-75.604,73.002,1 +33.954,-26.498,-33.513,1 +-17.189,-7.2439,14.947,1 +32.967,-16.065,-23.726,1 +72.961,44.831,-49.289,1 +-33.185,-8.0553,34.933,1 +-15.108,-76.07,-52.728,1 +14.449,72.121,34.645,1 +-8.4355,3.679,131.2,1 +-117.39,-36.299,35.86,1 +-8.9436,45.975,-45.118,1 +-19.972,4.7017,-2.7868,1 +48.912,148.25,-0.69856,1 +6.4792,-61.01,90.153,1 +8.7767,132.01,81.223,1 +16.49,-7.2297,-114.41,1 +-43.34,20.354,-14.248,1 +-38.423,78.704,-59.545,1 +-61.023,-6.5491,9.7717,1 +9.1325,76.983,-2.1497,1 +-36.221,-76.489,73.002,1 +33.871,-25.579,-33.513,1 +-17.746,-7.6354,14.947,1 +32.618,-15.175,-23.726,1 +71.046,46.721,-49.289,1 +-33.661,-8.9406,34.933,1 +-13.653,-76.412,-52.728,1 +11.669,72.56,34.645,1 +-9.2558,3.4943,131.2,1 +-117.03,-39.572,35.86,1 +-10.969,45.759,-45.118,1 +-20.816,4.1878,-2.7868,1 +43.949,149.65,-0.69856,1 +7.4962,-60.744,90.153,1 +4.2926,132.26,81.223,1 +15.971,-6.6997,-114.41,1 +-44.621,19.168,-14.248,1 +-41.369,77.634,-59.545,1 +-61.53,-8.2282,9.7717,1 +6.2161,77.269,-2.1497,1 +-34.746,-77.433,73.002,1 +33.867,-24.546,-33.513,1 +-18.24,-8.0808,14.947,1 +32.319,-14.182,-23.726,1 +68.967,48.784,-49.289,1 +-34.054,-9.9248,34.933,1 +-11.93,-76.733,-52.728,1 +8.6869,72.963,34.645,1 +-10.052,3.2725,131.2,1 +-116.41,-43.167,35.86,1 +-13.095,45.462,-45.118,1 +-21.629,3.6016,-2.7868,1 +38.523,151.03,-0.69856,1 +8.7148,-60.406,90.153,1 +-0.5658,132.4,81.223,1 +15.483,-6.122,-114.41,1 +-45.893,17.824,-14.248,1 +-44.484,76.364,-59.545,1 +-61.931,-10.091,9.7717,1 +3.0887,77.498,-2.1497,1 +-32.981,-78.418,73.002,1 +33.932,-23.396,-33.513,1 +-18.667,-8.5799,14.947,1 +32.058,-13.086,-23.726,1 +66.706,51.003,-49.289,1 +-34.354,-11.006,34.933,1 +-9.9394,-77.011,-52.728,1 +5.5019,73.307,34.645,1 +-10.822,3.011,131.2,1 +-115.52,-47.07,35.86,1 +-15.319,45.071,-45.118,1 +-22.403,2.9407,-2.7868,1 +32.628,152.35,-0.69856,1 +10.13,-59.982,90.153,1 +-5.7955,132.39,81.223,1 +15.022,-5.497,-114.41,1 +-47.144,16.318,-14.248,1 +-47.755,74.872,-59.545,1 +-62.209,-12.133,9.7717,1 +-0.24937,77.646,-2.1497,1 +-30.919,-79.421,73.002,1 +34.056,-22.125,-33.513,1 +-19.024,-9.1314,14.947,1 +31.828,-11.885,-23.726,1 +64.244,53.361,-49.289,1 +-34.553,-12.182,34.933,1 +-7.6813,-77.225,-52.728,1 +2.1135,73.566,34.645,1 +-11.563,2.7077,131.2,1 +-114.31,-51.263,35.86,1 +-17.634,44.569,-45.118,1 +-23.134,2.2032,-2.7868,1 +26.257,153.57,-0.69856,1 +11.736,-59.456,90.153,1 +-11.391,132.18,81.223,1 +14.581,-4.8254,-114.41,1 +-48.359,14.644,-14.248,1 +-51.165,73.133,-59.545,1 +-62.347,-14.353,9.7717,1 +-3.7964,77.686,-2.1497,1 +-28.556,-80.42,73.002,1 +34.226,-20.728,-33.513,1 +-19.304,-9.7337,14.947,1 +31.615,-10.579,-23.726,1 +61.562,55.836,-49.289,1 +-34.642,-13.448,34.933,1 +-5.1583,-77.35,-52.728,1 +-1.4768,73.715,34.645,1 +-12.274,2.3606,131.2,1 +-112.75,-55.726,35.86,1 +-20.034,43.942,-45.118,1 +-23.814,1.3881,-2.7868,1 +19.407,154.63,-0.69856,1 +13.523,-58.809,90.153,1 +-17.344,131.74,81.223,1 +14.155,-4.1077,-114.41,1 +-49.522,12.797,-14.248,1 +-54.694,71.125,-59.545,1 +-62.325,-16.742,9.7717,1 +-7.5487,77.592,-2.1497,1 +-25.886,-81.387,73.002,1 +34.427,-19.202,-33.513,1 +-19.504,-10.384,14.947,1 +31.407,-9.1669,-23.726,1 +58.641,58.406,-49.289,1 +-34.609,-14.798,34.933,1 +-2.3744,-77.357,-52.728,1 +-5.2657,73.724,34.645,1 +-12.949,1.9682,131.2,1 +-110.81,-60.433,35.86,1 +-22.511,43.173,-45.118,1 +-24.437,0.49467,-2.7868,1 +12.08,155.48,-0.69856,1 +15.483,-58.021,90.153,1 +-23.641,131,81.223,1 +13.735,-3.3448,-114.41,1 +-50.616,10.775,-14.248,1 +-58.32,68.823,-59.545,1 +-62.122,-19.293,9.7717,1 +-11.5,77.334,-2.1497,1 +-22.907,-82.292,73.002,1 +34.644,-17.543,-33.513,1 +-19.619,-11.079,14.947,1 +31.191,-7.6489,-23.726,1 +55.461,61.045,-49.289,1 +-34.444,-16.226,34.933,1 +0.66351,-77.219,-52.728,1 +-9.2474,73.565,34.645,1 +-13.587,1.5292,131.2,1 +-108.44,-65.353,35.86,1 +-25.054,42.245,-45.118,1 +-24.994,-0.47669,-2.7868,1 +4.2791,156.03,-0.69856,1 +17.603,-57.074,90.153,1 +-30.265,129.93,81.223,1 +13.316,-2.5376,-114.41,1 +-51.623,8.5754,-14.248,1 +-62.016,66.203,-59.545,1 +-61.719,-21.995,9.7717,1 +-15.643,76.881,-2.1497,1 +-19.618,-83.103,73.002,1 +34.862,-15.748,-33.513,1 +-19.643,-11.814,14.947,1 +30.953,-6.0257,-23.726,1 +52.003,63.722,-49.289,1 +-34.136,-17.722,34.933,1 +3.9462,-76.904,-52.728,1 +-13.413,73.205,34.645,1 +-14.184,1.043,131.2,1 +-105.61,-70.451,35.86,1 +-27.651,41.141,-45.118,1 +-25.477,-1.5249,-2.7868,1 +-3.984,156.24,-0.69856,1 +19.868,-55.945,90.153,1 +-37.194,128.46,81.223,1 +12.888,-1.6878,-114.41,1 +-52.521,6.1985,-14.248,1 +-65.753,63.241,-59.545,1 +-61.092,-24.834,9.7717,1 +-19.965,76.201,-2.1497,1 +-16.019,-83.784,73.002,1 +35.061,-13.816,-33.513,1 +-19.572,-12.583,14.947,1 +30.676,-4.2989,-23.726,1 +48.249,66.405,-49.289,1 +-33.675,-19.277,34.933,1 +7.4608,-76.379,-52.728,1 +-17.752,72.612,34.645,1 +-14.734,0.50924,131.2,1 +-102.28,-75.683,35.86,1 +-30.287,39.846,-45.118,1 +-25.877,-2.6477,-2.7868,1 +-12.693,156.03,-0.69856,1 +22.261,-54.613,90.153,1 +-44.399,126.54,81.223,1 +12.443,-0.79713,-114.41,1 +-53.29,3.6458,-14.248,1 +-69.497,59.915,-59.545,1 +-60.22,-27.793,9.7717,1 +-24.452,75.26,-2.1497,1 +-12.116,-84.297,73.002,1 +35.222,-11.745,-33.513,1 +-19.4,-13.381,14.947,1 +30.343,-2.4712,-23.726,1 +44.182,69.057,-49.289,1 +-33.049,-20.878,34.933,1 +11.191,-75.61,-52.728,1 +-22.247,71.753,34.645,1 +-15.235,-0.071632,131.2,1 +-98.42,-81.003,35.86,1 +-32.946,38.342,-45.118,1 +-26.185,-3.8418,-2.7868,1 +-21.824,155.32,-0.69856,1 +24.761,-53.057,90.153,1 +-51.846,124.12,81.223,1 +11.973,0.13157,-114.41,1 +-53.905,0.92141,-14.248,1 +-73.209,56.205,-59.545,1 +-59.081,-30.854,9.7717,1 +-29.084,74.025,-2.1497,1 +-7.9164,-84.602,73.002,1 +35.325,-9.5362,-33.513,1 +-19.123,-14.199,14.947,1 +29.939,-0.54698,-23.726,1 +39.788,71.639,-49.289,1 diff --git a/include/steam/evaluator/blockauto/transform/ConstAccTransformEvaluator.hpp b/include/steam/evaluator/blockauto/transform/ConstAccTransformEvaluator.hpp new file mode 100644 index 0000000..d61644e --- /dev/null +++ b/include/steam/evaluator/blockauto/transform/ConstAccTransformEvaluator.hpp @@ -0,0 +1,120 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file ConstAccTransformEvaluator.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_CONSTANT_ACC_TRANSFORM_EVALUATOR_HPP +#define STEAM_CONSTANT_ACC_TRANSFORM_EVALUATOR_HPP + +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Simple transform evaluator for a constant velocity model +////////////////////////////////////////////////////////////////////////////////////////////// +class ConstAccTransformEvaluator : public TransformEvaluator +{ + public: + + /// Convenience typedefs + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + ConstAccTransformEvaluator(const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Time& time); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Pseudo constructor - return a shared pointer to a new instance + ////////////////////////////////////////////////////////////////////////////////////////////// + static Ptr MakeShared(const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Time& time); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Adds references (shared pointers) to active state variables to the map output + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual void getActiveStateVariables( + std::map* outStates) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the transformation matrix + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual lgmath::se3::Transformation evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the transformation matrix tree + /// + /// ** Note that the returned pointer belongs to the memory pool EvalTreeNode::pool, + /// and should be given back to the pool, rather than being deleted. + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual EvalTreeNode* evaluateTree() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the Jacobian tree + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual void appendBlockAutomaticJacobians(const Eigen::MatrixXd& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Implementation for Block Automatic Differentiation + ////////////////////////////////////////////////////////////////////////////////////////////// + template + void appendJacobiansImpl(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Velocity state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + VectorSpaceStateVar::Ptr velocity_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Acceleration state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + VectorSpaceStateVar::Ptr acceleration_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Duration + ////////////////////////////////////////////////////////////////////////////////////////////// + Time time_; +}; + +} // se3 +} // steam + +#endif // STEAM_CONSTANT_ACC_TRANSFORM_EVALUATOR_HPP diff --git a/include/steam/evaluator/samples/ImuErrorEval.hpp b/include/steam/evaluator/samples/ImuErrorEval.hpp new file mode 100644 index 0000000..6fcdee4 --- /dev/null +++ b/include/steam/evaluator/samples/ImuErrorEval.hpp @@ -0,0 +1,76 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file ImuErrorEval.hpp +/// +/// \author Sean Anderson, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_IMU_ERROR_EVALUATOR_HPP +#define STEAM_IMU_ERROR_EVALUATOR_HPP + +#include + +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Error evaluator for a measured vector space state variable +////////////////////////////////////////////////////////////////////////////////////////////// +class ImuErrorEval : public ErrorEvaluator<6,6>::type +{ +public: + + /// Convenience typedefs + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + ImuErrorEval(const Eigen::Matrix& measurement, + const Eigen::Matrix3d& C_body_enu, + const VectorSpaceStateVar::ConstPtr& varpi, + const VectorSpaceStateVar::ConstPtr& varpi_dot, + const VectorSpaceStateVar::ConstPtr& imu_bias, + const lgmath::se3::Transformation& T_s_v); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the measurement error + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Matrix evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the measurement error and relevant Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Matrix evaluate( + const Eigen::Matrix& lhs, + std::vector >* jacs) const; + +private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Measurement vector + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::Matrix meas_; + + //////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Vectorspace state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + VectorSpaceStateVar::ConstPtr varpi_; + VectorSpaceStateVar::ConstPtr varpi_dot_; + VectorSpaceStateVar::ConstPtr imu_bias_; + + Eigen::Matrix3d C_body_enu_; + + Eigen::Vector3d gravity_; + + Eigen::Matrix adT_s_v_; + +}; + +} // steam + +#endif // STEAM_IMU_ERROR_EVALUATOR_HPP diff --git a/include/steam/evaluator/samples/LinearFuncErrorEval-inl.hpp b/include/steam/evaluator/samples/LinearFuncErrorEval-inl.hpp new file mode 100644 index 0000000..3120d3c --- /dev/null +++ b/include/steam/evaluator/samples/LinearFuncErrorEval-inl.hpp @@ -0,0 +1,69 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file LinearFuncErrorEval-inl.hpp +/// +/// \author Sean Anderson, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +template +LinearFuncErrorEval::LinearFuncErrorEval( + const Eigen::Matrix& measurement, + const Eigen::Matrix& C, + const VectorSpaceStateVar::ConstPtr& stateVec) + : measurement_(measurement), C_(C), stateVec_(stateVec) { +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +template +bool LinearFuncErrorEval::isActive() const { + return !stateVec_->isLocked(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the measurement error +////////////////////////////////////////////////////////////////////////////////////////////// +template +Eigen::Matrix LinearFuncErrorEval::evaluate() const { + return measurement_ - C_*(stateVec_->getValue()); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the measurement error and relevant Jacobians +////////////////////////////////////////////////////////////////////////////////////////////// +template +Eigen::Matrix LinearFuncErrorEval::evaluate( + const Eigen::Matrix& lhs, + std::vector >* jacs) const { + + // Check and initialize jacobian array + if (jacs == NULL) { + throw std::invalid_argument("Null pointer provided to return-input 'jacs' in evaluate"); + } + jacs->clear(); + + // Check that dimensions match + // if (lhs.cols() != stateVec_->getPerturbDim()) { + // throw std::runtime_error("evaluate had dimension mismatch."); + // } + + // Construct Jacobian + if(!stateVec_->isLocked()) { + jacs->resize(1); + Jacobian& jacref = jacs->back(); + jacref.key = stateVec_->getKey(); + jacref.jac = -lhs*C_; + } + + // Return error + return measurement_ - C_*(stateVec_->getValue()); +} + +} // steam diff --git a/include/steam/evaluator/samples/LinearFuncErrorEval.hpp b/include/steam/evaluator/samples/LinearFuncErrorEval.hpp new file mode 100644 index 0000000..f5b8751 --- /dev/null +++ b/include/steam/evaluator/samples/LinearFuncErrorEval.hpp @@ -0,0 +1,76 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file LinearFuncErrorEval.hpp +/// +/// \author Sean Anderson, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_LINEAR_FUNC_ERROR_EVALUATOR_HPP +#define STEAM_LINEAR_FUNC_ERROR_EVALUATOR_HPP + +#include + +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Error evaluator for a measured vector space state variable +////////////////////////////////////////////////////////////////////////////////////////////// +template +class LinearFuncErrorEval : public ErrorEvaluator::type +{ +public: + + /// Convenience typedefs + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + LinearFuncErrorEval(const Eigen::Matrix& measurement, + const Eigen::Matrix& C, + const VectorSpaceStateVar::ConstPtr& stateVec); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the measurement error + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Matrix evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the measurement error and relevant Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Matrix evaluate( + const Eigen::Matrix& lhs, + std::vector >* jacs) const; + +private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Measurement vector + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::Matrix measurement_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Vectorspace state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + VectorSpaceStateVar::ConstPtr stateVec_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Matrix that transforms state vector into measurement + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::Matrix C_; + + +}; + +typedef LinearFuncErrorEval LinearFuncErrorEvalX; + +} // steam + +#include + +#endif // STEAM_VECTOR_SPACE_ERROR_EVALUATOR_HPP diff --git a/include/steam/evaluator/samples/PointToPointErrorEval.hpp b/include/steam/evaluator/samples/PointToPointErrorEval.hpp new file mode 100644 index 0000000..c5e0279 --- /dev/null +++ b/include/steam/evaluator/samples/PointToPointErrorEval.hpp @@ -0,0 +1,87 @@ +// vim: ts=4:sw=4:noexpandtab +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file PointToPointErrorEval.hpp +/// +/// \author Francois Pomerleau, ASRL +/// \brief This evaluator was develop in the context of ICP (Iterative Closest Point) +/// implementation. +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_POINT_TO_POINT_ERROR_EVALUATOR_HPP +#define STEAM_POINT_TO_POINT_ERROR_EVALUATOR_HPP + +#include + +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief The distance between two points living in their respective frame is used as our +/// error function. +/// +////////////////////////////////////////////////////////////////////////////////////////////// +class PointToPointErrorEval : public ErrorEvaluator<4,6>::type +{ +public: + + /// Convenience typedefs + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + /// \param ref_a A point from the reference point cloud (static) expressed in homogeneous + /// coordinates (i.e., [x, y, z, 1]) and in the frame a. + /// \param T_a_world Transformation matrix from frame world to frame a. + /// \param read_b A point from the reading point cloud expressed in homogeneous + /// coordinates (i.e., [x, y, z, 1]) and in the frame b. + /// \param T_b_world Transformation matrix from frame world to frame b. + ////////////////////////////////////////////////////////////////////////////////////////////// + PointToPointErrorEval(const Eigen::Vector4d& ref_a, + const se3::TransformEvaluator::ConstPtr& T_a_world, + const Eigen::Vector4d& read_b, + const se3::TransformEvaluator::ConstPtr& T_b_world + ); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + /// \param ref_a A point from the reference point cloud (static) expressed in homogeneous + /// coordinates (i.e., [x, y, z, 1]) and in the frame a. + /// \param read_b A point from the reading point cloud expressed in homogeneous + /// coordinates (i.e., [x, y, z, 1]) and in the frame b. + /// \param T_a_b Transformation matrix from frame b to frame a. + ////////////////////////////////////////////////////////////////////////////////////////////// + PointToPointErrorEval(const Eigen::Vector4d& ref_a, + const Eigen::Vector4d& read_b, + const se3::TransformEvaluator::ConstPtr& T_b_a + ); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; //TODO: check if we need to define that + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the 4-d measurement error (x, y, z, 0) + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Vector4d evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the 4-d measurement error (x, y, z, 0) and Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::Vector4d evaluate(const Eigen::Matrix4d& lhs, + std::vector >* jacs) const; + +private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Point evaluator (evaluates the point transformed into the camera frame) + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::Vector4d ref_a_; + se3::TransformEvaluator::ConstPtr T_b_a_; + Eigen::Vector4d read_b_; + +}; + +} // steam + +#endif // STEAM_POINT_TO_POINT_ERROR_EVALUATOR_HPP diff --git a/include/steam/trajectory/SteamCATrajInterface.hpp b/include/steam/trajectory/SteamCATrajInterface.hpp new file mode 100644 index 0000000..dbc22db --- /dev/null +++ b/include/steam/trajectory/SteamCATrajInterface.hpp @@ -0,0 +1,90 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajInterface.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_CA_TRAJECTORY_INTERFACE_HPP +#define STEAM_CA_TRAJECTORY_INTERFACE_HPP + +#include + +#include + +#include +#include + +#include +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief The trajectory class wraps a set of state variables to provide an interface +/// that allows for continuous-time pose interpolation. +////////////////////////////////////////////////////////////////////////////////////////////// +class SteamCATrajInterface : public SteamTrajInterface +{ + public: + + using SteamTrajInterface::SteamTrajInterface; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Add a new knot + ////////////////////////////////////////////////////////////////////////////////////////////// + void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration); + + void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get evaluator + ////////////////////////////////////////////////////////////////////////////////////////////// + TransformEvaluator::ConstPtr getInterpPoseEval(const steam::Time& time) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get velocity evaluator + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::VectorXd getVelocity(const steam::Time& time); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get acceleration evaluator + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::VectorXd getAcceleration(const steam::Time& time); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Add a unary acceleration prior factor at a knot time. Note that only a single acceleration + /// prior should exist on a trajectory, adding a second will overwrite the first. + ////////////////////////////////////////////////////////////////////////////////////////////// + void addAccelerationPrior(const steam::Time& time, const Eigen::Matrix& acceleration, + const Eigen::Matrix& cov); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get binary cost terms associated with the prior for active parts of the trajectory + ////////////////////////////////////////////////////////////////////////////////////////////// + void appendPriorCostTerms(const ParallelizedCostTermCollection::Ptr& costTerms) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get interpolated/extrapolated covariance at given time + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::MatrixXd getCovariance(const steam::Time& time) const; + + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Acceleration prior + ////////////////////////////////////////////////////////////////////////////////////////////// + steam::WeightedLeastSqCostTerm<6,6>::Ptr accelerationPriorFactor_; + +}; + +} // se3 +} // steam + +#endif // STEAM_CA_TRAJECTORY_INTERFACE_HPP diff --git a/include/steam/trajectory/SteamCATrajPoseInterpEval.hpp b/include/steam/trajectory/SteamCATrajPoseInterpEval.hpp new file mode 100644 index 0000000..9974c32 --- /dev/null +++ b/include/steam/trajectory/SteamCATrajPoseInterpEval.hpp @@ -0,0 +1,139 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajPoseInterpEval.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_CA_TRAJECTORY_POSE_INTERP_EVAL_HPP +#define STEAM_CA_TRAJECTORY_POSE_INTERP_EVAL_HPP + +#include + +#include +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Simple transform evaluator for a transformation state variable +////////////////////////////////////////////////////////////////////////////////////////////// +class SteamCATrajPoseInterpEval : public TransformEvaluator +{ + public: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajPoseInterpEval(const Time& time, + const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Pseudo constructor - return a shared pointer to a new instance + ////////////////////////////////////////////////////////////////////////////////////////////// + static Ptr MakeShared(const Time& time, + const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Adds references (shared pointers) to active state variables to the map output + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual void getActiveStateVariables( + std::map* outStates) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the transformation matrix + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual lgmath::se3::Transformation evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the transformation matrix tree + /// + /// ** Note that the returned pointer belongs to the memory pool EvalTreeNode::pool, + /// and should be given back to the pool, rather than being deleted. + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual EvalTreeNode* evaluateTree() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the Jacobian tree + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual void appendBlockAutomaticJacobians( + const Eigen::MatrixXd& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + virtual void appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Implementation for Block Automatic Differentiation + ////////////////////////////////////////////////////////////////////////////////////////////// + template + void appendJacobiansImpl(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief First (earlier) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamTrajVar::ConstPtr knot1_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Second (later) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamTrajVar::ConstPtr knot2_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Interpolation coefficients + ////////////////////////////////////////////////////////////////////////////////////////////// + double omega11_; + double omega12_; + double omega13_; + + double lambda12_; + double lambda13_; + + // double psi11_; + // double psi12_; + // double psi21_; + // double psi22_; + // double lambda11_; + // double lambda12_; + // double lambda21_; + // double lambda22_; +}; + +} // se3 +} // steam + +#endif // STEAM_CA_TRAJECTORY_POSE_INTERP_EVAL_HPP diff --git a/include/steam/trajectory/SteamCATrajPriorFactor.hpp b/include/steam/trajectory/SteamCATrajPriorFactor.hpp new file mode 100644 index 0000000..7738e0a --- /dev/null +++ b/include/steam/trajectory/SteamCATrajPriorFactor.hpp @@ -0,0 +1,67 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajPriorFactor.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_CA_TRAJECTORY_PRIOR_FACTOR_HPP +#define STEAM_CA_TRAJECTORY_PRIOR_FACTOR_HPP + +#include + +#include +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Gaussian-process prior evaluator +////////////////////////////////////////////////////////////////////////////////////////////// +class SteamCATrajPriorFactor : public ErrorEvaluatorX +{ + public: + + /// Shared pointer typedefs for readability + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajPriorFactor(const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the GP prior factor + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::VectorXd evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the GP prior factor and Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::VectorXd evaluate(const Eigen::MatrixXd& lhs, std::vector >* jacs) const; + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief First (earlier) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamTrajVar::ConstPtr knot1_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Second (later) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamTrajVar::ConstPtr knot2_; +}; + +} // se3 +} // steam + +#endif // STEAM_CA_TRAJECTORY_PRIOR_FACTOR_HPP + diff --git a/include/steam/trajectory/SteamCATrajVar.hpp b/include/steam/trajectory/SteamCATrajVar.hpp new file mode 100644 index 0000000..1de5d9d --- /dev/null +++ b/include/steam/trajectory/SteamCATrajVar.hpp @@ -0,0 +1,61 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajVar.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_CA_TRAJECTORY_VAR_HPP +#define STEAM_CA_TRAJECTORY_VAR_HPP + +#include + +#include +#include +#include + +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief This class wraps a pose and velocity evaluator to act as a discrete-time trajectory +/// state variable for continuous-time trajectory estimation. +////////////////////////////////////////////////////////////////////////////////////////////// +class SteamCATrajVar : public SteamTrajVar +{ + public: + + /// Shared pointer typedefs for readability + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCATrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration); + + SteamCATrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get velocity state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + const VectorSpaceStateVar::Ptr& getAcceleration() const; + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Generalized 6D acceleration state variable + ////////////////////////////////////////////////////////////////////////////////////////////// + VectorSpaceStateVar::Ptr acceleration_; +}; + +} // se3 +} // steam + +#endif // STEAM_CA_TRAJECTORY_VAR_HPP diff --git a/include/steam/trajectory/SteamCustomPriorFactor.hpp b/include/steam/trajectory/SteamCustomPriorFactor.hpp new file mode 100644 index 0000000..c63f697 --- /dev/null +++ b/include/steam/trajectory/SteamCustomPriorFactor.hpp @@ -0,0 +1,67 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamTrajPriorFactor.hpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#ifndef STEAM_CUSTOM_PRIOR_FACTOR_HPP +#define STEAM_CUSTOM_PRIOR_FACTOR_HPP + +#include + +#include +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Gaussian-process prior evaluator +////////////////////////////////////////////////////////////////////////////////////////////// +class SteamCustomPriorFactor : public ErrorEvaluatorX +{ + public: + + /// Shared pointer typedefs for readability + typedef boost::shared_ptr Ptr; + typedef boost::shared_ptr ConstPtr; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Constructor + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamCustomPriorFactor(const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Returns whether or not an evaluator contains unlocked state variables + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual bool isActive() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the GP prior factor + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::VectorXd evaluate() const; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Evaluate the GP prior factor and Jacobians + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual Eigen::VectorXd evaluate(const Eigen::MatrixXd& lhs, std::vector >* jacs) const; + + private: + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief First (earlier) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamTrajVar::ConstPtr knot1_; + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Second (later) knot + ////////////////////////////////////////////////////////////////////////////////////////////// + SteamTrajVar::ConstPtr knot2_; +}; + +} // se3 +} // steam + +#endif // STEAM_CUSTOM_PRIOR_FACTOR_HPP + diff --git a/include/steam/trajectory/SteamTrajInterface.hpp b/include/steam/trajectory/SteamTrajInterface.hpp index 67bfacd..7474d93 100644 --- a/include/steam/trajectory/SteamTrajInterface.hpp +++ b/include/steam/trajectory/SteamTrajInterface.hpp @@ -16,6 +16,8 @@ #include #include +#include + namespace steam { namespace se3 { @@ -50,15 +52,31 @@ class SteamTrajInterface void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, const VectorSpaceStateVar::Ptr& velocity); + void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const Eigen::Matrix cov); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Add a new knot + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration); + + virtual void add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov); + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get transform evaluator ////////////////////////////////////////////////////////////////////////////////////////////// - TransformEvaluator::ConstPtr getInterpPoseEval(const steam::Time& time) const; + virtual TransformEvaluator::ConstPtr getInterpPoseEval(const steam::Time& time) const; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get velocity evaluator ////////////////////////////////////////////////////////////////////////////////////////////// - Eigen::VectorXd getVelocity(const steam::Time& time); + virtual Eigen::VectorXd getVelocity(const steam::Time& time); ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a unary pose prior factor at a knot time. Note that only a single pose prior @@ -77,18 +95,35 @@ class SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get binary cost terms associated with the prior for active parts of the trajectory ////////////////////////////////////////////////////////////////////////////////////////////// - void appendPriorCostTerms(const ParallelizedCostTermCollection::Ptr& costTerms) const; + virtual void appendPriorCostTerms(const ParallelizedCostTermCollection::Ptr& costTerms) const; ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get active state variables in the trajectory ////////////////////////////////////////////////////////////////////////////////////////////// - void getActiveStateVariables( + virtual void getActiveStateVariables( std::map* outStates) const; + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get pose prior cost + ////////////////////////////////////////////////////////////////////////////////////////////// double getPosePriorCost(); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get velocity prior cost + ////////////////////////////////////////////////////////////////////////////////////////////// double getVelocityPriorCost(); - private: + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Store solver in trajectory, needed for querying covariances later + ////////////////////////////////////////////////////////////////////////////////////////////// + void setSolver(std::shared_ptr solver); + + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get interpolated/extrapolated covariance at given time + ////////////////////////////////////////////////////////////////////////////////////////////// + Eigen::MatrixXd getCovariance(const steam::Time& time) const; + + protected: ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Ordered map of knots @@ -115,6 +150,11 @@ class SteamTrajInterface ////////////////////////////////////////////////////////////////////////////////////////////// std::map knotMap_; + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Solver used, we use this to query covariances + ////////////////////////////////////////////////////////////////////////////////////////////// + std::shared_ptr solver_; + }; } // se3 diff --git a/include/steam/trajectory/SteamTrajVar.hpp b/include/steam/trajectory/SteamTrajVar.hpp index dcb5cf7..7c23076 100644 --- a/include/steam/trajectory/SteamTrajVar.hpp +++ b/include/steam/trajectory/SteamTrajVar.hpp @@ -34,6 +34,9 @@ class SteamTrajVar SteamTrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, const VectorSpaceStateVar::Ptr& velocity); + SteamTrajVar(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, const Eigen::Matrix cov); + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get pose evaluator ////////////////////////////////////////////////////////////////////////////////////////////// @@ -44,11 +47,20 @@ class SteamTrajVar ////////////////////////////////////////////////////////////////////////////////////////////// const VectorSpaceStateVar::Ptr& getVelocity() const; + ////////////////////////////////////////////////////////////////////////////////////////////// + /// \brief Get acceleration state variable, to be overriden + ////////////////////////////////////////////////////////////////////////////////////////////// + virtual const VectorSpaceStateVar::Ptr& getAcceleration() const; + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get timestamp ////////////////////////////////////////////////////////////////////////////////////////////// const steam::Time& getTime() const; + const Eigen::MatrixXd getCovariance() const; + + bool covarianceSet() const; + private: ////////////////////////////////////////////////////////////////////////////////////////////// @@ -65,6 +77,10 @@ class SteamTrajVar /// \brief Generalized 6D velocity state variable ////////////////////////////////////////////////////////////////////////////////////////////// VectorSpaceStateVar::Ptr velocity_; + + protected: + Eigen::MatrixXd cov_; + bool cov_set_=false; }; } // se3 diff --git a/samples/CMakeLists.txt b/samples/CMakeLists.txt index 2943546..a579335 100644 --- a/samples/CMakeLists.txt +++ b/samples/CMakeLists.txt @@ -40,3 +40,15 @@ target_link_libraries(SimpleTrajectoryPrior steam ${DEPEND_LIBS}) # simple example of trajectory interface for prior, combined with bundle adjustment add_executable(SimpleBAandTrajPrior ${CMAKE_CURRENT_SOURCE_DIR}/SimpleBAandTrajPrior.cpp) target_link_libraries(SimpleBAandTrajPrior steam ${DEPEND_LIBS}) + +# simple example of constant acceleration trajectory interface for prior, combined with bundle adjustment +add_executable(SimpleBAandCATrajPrior ${CMAKE_CURRENT_SOURCE_DIR}/SimpleBAandCATrajPrior.cpp) +target_link_libraries(SimpleBAandCATrajPrior steam ${DEPEND_LIBS}) + +# simple example of constant acceleration trajectory interface for prior, combined with p2p error terms +add_executable(SimpleP2PandCATrajPrior ${CMAKE_CURRENT_SOURCE_DIR}/SimpleP2PandCATrajPrior.cpp) +target_link_libraries(SimpleP2PandCATrajPrior steam ${DEPEND_LIBS}) + +# simple example of constant acceleration trajectory interface for prior, combined with p2p error terms +add_executable(MotionDistortedP2PandCATrajPrior ${CMAKE_CURRENT_SOURCE_DIR}/MotionDistortedP2PandCATrajPrior.cpp) +target_link_libraries(MotionDistortedP2PandCATrajPrior steam ${DEPEND_LIBS}) diff --git a/samples/MotionDistortedP2PandCATrajPrior.cpp b/samples/MotionDistortedP2PandCATrajPrior.cpp new file mode 100644 index 0000000..e77bb96 --- /dev/null +++ b/samples/MotionDistortedP2PandCATrajPrior.cpp @@ -0,0 +1,236 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file MotionDistortedP2PandCATrajPrior.cpp +/// \brief A sample usage of the STEAM Engine library for a bundle adjustment problem +/// with point-to-point error terms and trajectory smoothing factors. +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include +#include +#include +#include + +using namespace std; +typedef steam::PointToPointErrorEval Error; +typedef steam::WeightedLeastSqCostTerm<4,6> Cost; +unsigned int M = 20; +unsigned int N = 20; + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Structure to store trajectory state variables +////////////////////////////////////////////////////////////////////////////////////////////// +struct TrajStateVar { + steam::Time time; + steam::se3::TransformStateVar::Ptr pose; + steam::VectorSpaceStateVar::Ptr velocity; + steam::VectorSpaceStateVar::Ptr acceleration; +}; + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Example that loads and solves simple bundle adjustment problems +////////////////////////////////////////////////////////////////////////////////////////////// +int main(int argc, char** argv) { + + /// + /// Setup Traj Prior + /// + + + // Smoothing factor diagonal -- in this example, we penalize accelerations in each dimension + // except for the forward and yaw (this should be fairly typical) + double lin_acc_stddev_x = 1.00; // body-centric (e.g. x is usually forward) + double lin_acc_stddev_y = 0.01; // body-centric (e.g. y is usually side-slip) + double lin_acc_stddev_z = 0.01; // body-centric (e.g. z is usually 'jump') + double ang_acc_stddev_x = 0.01; // ~roll + double ang_acc_stddev_y = 0.01; // ~pitch + double ang_acc_stddev_z = 1.00; // ~yaw + Eigen::Array Qc_diag; + Qc_diag << lin_acc_stddev_x, lin_acc_stddev_y, lin_acc_stddev_z, + ang_acc_stddev_x, ang_acc_stddev_y, ang_acc_stddev_z; + Qc_diag = Qc_diag * 1; + + // Make Qc_inv + Eigen::Matrix Qc_inv; Qc_inv.setZero(); + Qc_inv.diagonal() = 1.0/Qc_diag; + + /// + /// Parse Dataset + /// + + string filePath = "../../include/steam/data/PointToPoint_distorted/points_simulated.txt"; + ifstream in(filePath); + + vector> fields; + int count = 0; + if (in) { + string line; + while (getline(in, line)) { + count++; + stringstream sep(line); + string field; + fields.push_back(vector()); + while (getline(sep, field, ',')) { + fields.back().push_back(stod(field)); + } + } + } + + filePath = "../../include/steam/data/PointToPoint_distorted/poses_ic.txt"; + ifstream inIc(filePath); + + vector> fieldsIC; + count = 0; + if (inIc) { + string line; + while (getline(inIc, line)) { + count++; + stringstream sep(line); + string field; + fieldsIC.push_back(vector()); + while (getline(sep, field, ',')) { + fieldsIC.back().push_back(stod(field)); + } + } + } + + cout << fieldsIC.size() << endl; + cout << fieldsIC.back().front() << endl; + + // State variable containers (and related data) + std::vector traj_states_ic; + + // Zero velocity + Eigen::Matrix initVelocity; initVelocity.setZero(); + // initVelocity << -10.0, 0, 0, 0, 0, 0; + + // Zero acceleration + Eigen::Matrix initAcceleration; initAcceleration.setZero(); + // initAcceleration << -3.0, 0, 0, 0, 0, 0; + + // Setup state variables using initial condition + for (unsigned int i = 0; i < N; i++) { + TrajStateVar temp; + double dt = double(i-0)*0.1; + temp.time = steam::Time(dt); + Eigen::Matrix T_k0; + T_k0 << fieldsIC[i][0], fieldsIC[i][1], fieldsIC[i][2], fieldsIC[i][3], + fieldsIC[i][4], fieldsIC[i][5], fieldsIC[i][6], fieldsIC[i][7], + fieldsIC[i][8], fieldsIC[i][9], fieldsIC[i][10], fieldsIC[i][11], + 0.0, 0.0, 0.0, 1; + + lgmath::se3::Transformation T_k0_tf(T_k0); + + temp.pose = steam::se3::TransformStateVar::Ptr(new steam::se3::TransformStateVar(T_k0_tf)); + // initVelocity << fieldsIC[i+20][0],fieldsIC[i+20][1],fieldsIC[i+20][2],fieldsIC[i+20][3],fieldsIC[i+20][4],fieldsIC[i+20][5]; + // initAcceleration << fieldsIC[i+20][6],fieldsIC[i+20][7],fieldsIC[i+20][8],fieldsIC[i+20][9],fieldsIC[i+20][10],fieldsIC[i+20][11]; + + temp.velocity = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initVelocity)); + temp.acceleration = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initAcceleration)); + traj_states_ic.push_back(temp); + } + + // Setup Trajectory + steam::se3::SteamCATrajInterface traj(Qc_inv); + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + TrajStateVar& state = traj_states_ic.at(i); + steam::se3::TransformStateEvaluator::Ptr temp = + steam::se3::TransformStateEvaluator::MakeShared(state.pose); + traj.add(state.time, temp, state.velocity, state.acceleration); + } + + // Lock first pose (otherwise entire solution is 'floating') + // **Note: alternatively we could add a prior to the first pose. + traj_states_ic[0].pose->setLock(true); + + + // steam cost terms + steam::ParallelizedCostTermCollection::Ptr measCostTerms(new steam::ParallelizedCostTermCollection()); + + // Setup shared noise and loss function + Eigen::Matrix4d measurementNoise; + measurementNoise.setIdentity(); + steam::BaseNoiseModel<4>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<4>(measurementNoise)); + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + Error::Ptr error; + Cost::Ptr cost; + + // cout << fields[1*N+1][1] << endl; + + for (unsigned int i = 1; i < N; i++) { + // steam::se3::TransformEvaluator::Ptr T_k0 = steam::se3::TransformStateEvaluator::MakeShared(traj_states_ic[i].pose); + + for (unsigned int j = 0; j < M; j++) { + double t = (i-1)*0.1 + double(j+1)/double(M)*0.1; + auto T_k0 = traj.getInterpPoseEval(t); + Eigen::Vector4d ref_a; + ref_a << fields[j][0] , fields[j][1] , fields[j][2] , fields[j][3]; + + Eigen::Vector4d read_b; + read_b << fields[i*N+j][0] , fields[i*N+j][1] , fields[i*N+j][2] , fields[i*N+j][3]; + + error.reset(new Error(ref_a, read_b, T_k0)); + cost.reset(new Cost(error, sharedNoiseModel, sharedLossFunc)); + measCostTerms->add(cost); + } + } + + // Trajectory prior smoothing terms + steam::ParallelizedCostTermCollection::Ptr smoothingCostTerms(new steam::ParallelizedCostTermCollection()); + traj.appendPriorCostTerms(smoothingCostTerms); + + /// + /// Make Optimization Problem + /// + + // Initialize problem + steam::OptimizationProblem problem; + + // Add state variables + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + const TrajStateVar& state = traj_states_ic.at(i); + problem.addStateVariable(state.pose); + problem.addStateVariable(state.velocity); + problem.addStateVariable(state.acceleration); + } + + // Add cost terms + problem.addCostTerm(measCostTerms); + problem.addCostTerm(smoothingCostTerms); + + /// + /// Setup Solver and Optimize + /// + typedef steam::VanillaGaussNewtonSolver SolverType; + + // Initialize parameters (enable verbose mode) + SolverType::Params params; + params.verbose = true; + params.absoluteCostChangeThreshold = 1e-20; + params.relativeCostChangeThreshold = 1e-20; + + // Make solver + SolverType solver(&problem, params); + + // Optimize + TrajStateVar& state = traj_states_ic.back(); + cout << state.velocity->getValue().transpose() << endl; + cout << state.acceleration->getValue().transpose() << endl; + + solver.optimize(); + + // Setup Trajectory + // for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + // TrajStateVar& state = traj_states_ic.at(i); + // std::cout << i << ": \n " << state.pose->getValue() << "\n"; + // } + + state = traj_states_ic.back(); + cout << state.pose->getValue() << endl; + cout << state.velocity->getValue().transpose() << endl; + cout << state.acceleration->getValue().transpose() << endl; + return 0; + +} diff --git a/samples/SimpleBAandCATrajPrior.cpp b/samples/SimpleBAandCATrajPrior.cpp new file mode 100644 index 0000000..b200954 --- /dev/null +++ b/samples/SimpleBAandCATrajPrior.cpp @@ -0,0 +1,291 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SimpleBAandCATrajPrior.cpp +/// \brief A sample usage of the STEAM Engine library for a bundle adjustment problem +/// with relative landmarks and trajectory smoothing factors. +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Structure to store trajectory state variables +////////////////////////////////////////////////////////////////////////////////////////////// +struct TrajStateVar { + steam::Time time; + steam::se3::TransformStateVar::Ptr pose; + steam::VectorSpaceStateVar::Ptr velocity; + steam::VectorSpaceStateVar::Ptr acceleration; +}; + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Example that loads and solves simple bundle adjustment problems +////////////////////////////////////////////////////////////////////////////////////////////// +int main(int argc, char** argv) { + + /// + /// Setup Traj Prior + /// + + + // Smoothing factor diagonal -- in this example, we penalize accelerations in each dimension + // except for the forward and yaw (this should be fairly typical) + double lin_acc_stddev_x = 1.00; // body-centric (e.g. x is usually forward) + double lin_acc_stddev_y = 0.01; // body-centric (e.g. y is usually side-slip) + double lin_acc_stddev_z = 0.01; // body-centric (e.g. z is usually 'jump') + double ang_acc_stddev_x = 0.01; // ~roll + double ang_acc_stddev_y = 0.01; // ~pitch + double ang_acc_stddev_z = 1.00; // ~yaw + Eigen::Array Qc_diag; + Qc_diag << lin_acc_stddev_x, lin_acc_stddev_y, lin_acc_stddev_z, + ang_acc_stddev_x, ang_acc_stddev_y, ang_acc_stddev_z; + + // Make Qc_inv + Eigen::Matrix Qc_inv; Qc_inv.setZero(); + Qc_inv.diagonal() = 1.0/Qc_diag; + + /// + /// Parse Dataset + /// + + // Get filename + std::string filename; + if (argc < 2) { + filename = "../../include/steam/data/stereo_simulated.txt"; + //filename = "../../include/steam/data/stereo_simulated_window1.txt"; + //filename = "../../include/steam/data/stereo_simulated_window2.txt"; + std::cout << "Parsing default file: " << filename << std::endl << std::endl; + } else { + filename = argv[1]; + std::cout << "Parsing file: " << filename << std::endl << std::endl; + } + + // Load dataset + steam::data::SimpleBaDataset dataset = steam::data::parseSimpleBaDataset(filename); + std::cout << "Problem has: " << dataset.frames_gt.size() << " poses" << std::endl; + std::cout << " " << dataset.frames_gt.size() << " velocities" << std::endl; + std::cout << " " << dataset.land_gt.size() << " landmarks" << std::endl; + std::cout << " ~" << double(dataset.meas.size())/dataset.frames_gt.size() << " meas per pose" << std::endl << std::endl; + + /// + /// Setup States + /// + + // Set a fixed identity transform that will be used to initialize landmarks in their parent frame + steam::se3::FixedTransformEvaluator::Ptr tf_identity = + steam::se3::FixedTransformEvaluator::MakeShared(lgmath::se3::Transformation()); + + // Fixed vehicle to camera transform + steam::se3::TransformEvaluator::Ptr tf_c_v = + steam::se3::FixedTransformEvaluator::MakeShared(dataset.T_cv); + + // Ground truth + std::vector poses_gt_k_0; + std::vector landmarks_gt; + + // State variable containers (and related data) + std::vector traj_states_ic; + //std::vector poses_ic_k_0; + std::vector landmarks_ic; + + // Record the frame in which the landmark is first seen in order to set up transforms correctly + std::map landmark_map; + + /// + /// Initialize States + /// + + // Setup ground-truth poses + for (unsigned int i = 0; i < dataset.frames_gt.size(); i++) { + steam::se3::TransformStateVar::Ptr temp(new steam::se3::TransformStateVar(dataset.frames_gt[i].T_k0)); + poses_gt_k_0.push_back(temp); + } + + // Setup ground-truth landmarks + for (unsigned int i = 0; i < dataset.land_gt.size(); i++) { + steam::se3::LandmarkStateVar::Ptr temp(new steam::se3::LandmarkStateVar(dataset.land_gt[i].point)); + landmarks_gt.push_back(temp); + } + + // Zero velocity + Eigen::Matrix initVelocity; initVelocity.setZero(); + + // Zero acceleration + Eigen::Matrix initAcceleration; initAcceleration.setZero(); + + // Setup state variables using initial condition + for (unsigned int i = 0; i < dataset.frames_ic.size(); i++) { + TrajStateVar temp; + temp.time = steam::Time(dataset.frames_ic[i].time); + temp.pose = steam::se3::TransformStateVar::Ptr(new steam::se3::TransformStateVar(dataset.frames_ic[i].T_k0)); + temp.velocity = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initVelocity)); + temp.acceleration = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initAcceleration)); + // std::cout << i << " : " << dataset.frames_ic[i].time << " " << dataset.frames_ic[i].T_k0; + traj_states_ic.push_back(temp); + } + + // Setup Trajectory + steam::se3::SteamCATrajInterface ca_traj(Qc_inv); + steam::se3::SteamTrajInterface &traj{ca_traj}; + + // steam::se3::SteamCATrajInterface traj(Qc_inv); + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + TrajStateVar& state = traj_states_ic.at(i); + steam::se3::TransformStateEvaluator::Ptr temp = + steam::se3::TransformStateEvaluator::MakeShared(state.pose); + steam::se3::SteamCATrajVar::Ptr traj_var(new steam::se3::SteamCATrajVar(state.time, temp, state.velocity, state.acceleration)); + traj.add(traj_var); + // traj.add(state.time, temp, state.velocity, state.acceleration); + + } + + // Lock first pose (otherwise entire solution is 'floating') + // **Note: alternatively we could add a prior to the first pose. + traj_states_ic[0].pose->setLock(true); + + // Lock the first velocity as well + // traj_states_ic[0].velocity->setLock(true); + + // Setup relative landmarks + landmarks_ic.resize(dataset.land_ic.size()); + for (unsigned int i = 0; i < dataset.meas.size(); i++) { + + // Get pose reference + unsigned int frameIdx = dataset.meas[i].frameID; + + // Get landmark reference + unsigned int landmarkIdx = dataset.meas[i].landID; + + // Setup landmark if first time + if (!landmarks_ic[landmarkIdx]) { + + // Get homogeneous point in inertial frame + Eigen::Vector4d p_0; + p_0.head<3>() = dataset.land_ic[landmarkIdx].point; + p_0[3] = 1.0; + + // Get transform between first observation time and inertial frame + lgmath::se3::Transformation pose_vk_0 = dataset.frames_ic[frameIdx].T_k0/dataset.frames_ic[0].T_k0; + + // Get point in 'local' frame + Eigen::Vector4d p_vehicle = pose_vk_0 * p_0; + + // Insert the landmark + landmarks_ic[landmarkIdx] = steam::se3::LandmarkStateVar::Ptr(new steam::se3::LandmarkStateVar(p_vehicle.head<3>())); + + // Keep a record of its 'parent' frame + landmark_map[landmarkIdx] = frameIdx; + } + } + + /// + /// Setup Cost Terms + /// + + // steam cost terms + steam::ParallelizedCostTermCollection::Ptr stereoCostTerms(new steam::ParallelizedCostTermCollection()); + + // Setup shared noise and loss function + steam::BaseNoiseModel<4>::Ptr sharedCameraNoiseModel(new steam::StaticNoiseModel<4>(dataset.noise)); + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + + // Setup camera intrinsics + steam::stereo::CameraIntrinsics::Ptr sharedIntrinsics( + new steam::stereo::CameraIntrinsics()); + sharedIntrinsics->b = dataset.camParams.b; + sharedIntrinsics->fu = dataset.camParams.fu; + sharedIntrinsics->fv = dataset.camParams.fv; + sharedIntrinsics->cu = dataset.camParams.cu; + sharedIntrinsics->cv = dataset.camParams.cv; + + // Generate cost terms for camera measurements + for (unsigned int i = 0; i < dataset.meas.size(); i++) { + + // Get pose reference + unsigned int frameIdx = dataset.meas[i].frameID; + + // Get landmark reference + unsigned int landmarkIdx = dataset.meas[i].landID; + steam::se3::LandmarkStateVar::Ptr& landVar = landmarks_ic[landmarkIdx]; + + // Construct transform evaluator between two vehicle frames (a and b) that have observations + steam::se3::TransformEvaluator::Ptr tf_vb_va; + if(landmark_map[landmarkIdx] == frameIdx) { + + // In this case, the transform remains fixed as an identity transform + tf_vb_va = tf_identity; + + } else { + + unsigned int firstObsIndex = landmark_map[landmarkIdx]; + steam::se3::TransformEvaluator::Ptr pose_va_0 = steam::se3::TransformStateEvaluator::MakeShared(traj_states_ic[firstObsIndex].pose); + steam::se3::TransformEvaluator::Ptr pose_vb_0 = steam::se3::TransformStateEvaluator::MakeShared(traj_states_ic[frameIdx].pose); + tf_vb_va = steam::se3::composeInverse(pose_vb_0, pose_va_0); + } + + // Compose with camera to vehicle transform + steam::se3::TransformEvaluator::Ptr tf_cb_va = steam::se3::compose(tf_c_v, tf_vb_va); + + // Construct error function + steam::StereoCameraErrorEval::Ptr errorfunc(new steam::StereoCameraErrorEval( + dataset.meas[i].data, sharedIntrinsics, tf_cb_va, landVar)); + + // Construct cost term + steam::WeightedLeastSqCostTerm<4,6>::Ptr cost(new steam::WeightedLeastSqCostTerm<4,6>(errorfunc, sharedCameraNoiseModel, sharedLossFunc)); + stereoCostTerms->add(cost); + } + + // Trajectory prior smoothing terms + steam::ParallelizedCostTermCollection::Ptr smoothingCostTerms(new steam::ParallelizedCostTermCollection()); + traj.appendPriorCostTerms(smoothingCostTerms); + + /// + /// Make Optimization Problem + /// + + // Initialize problem + steam::OptimizationProblem problem; + + // Add state variables + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + const TrajStateVar& state = traj_states_ic.at(i); + problem.addStateVariable(state.pose); + problem.addStateVariable(state.velocity); + problem.addStateVariable(state.acceleration); + } + + // Add landmark variables + for (unsigned int i = 0; i < landmarks_ic.size(); i++) { + problem.addStateVariable(landmarks_ic[i]); + } + + // Add cost terms + problem.addCostTerm(stereoCostTerms); + problem.addCostTerm(smoothingCostTerms); + + /// + /// Setup Solver and Optimize + /// + typedef steam::DoglegGaussNewtonSolver SolverType; + + // Initialize parameters (enable verbose mode) + SolverType::Params params; + params.verbose = true; + + // Make solver + SolverType solver(&problem, params); + + // Optimize + solver.optimize(); + + // Setup Trajectory + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + TrajStateVar& state = traj_states_ic.at(i); + std::cout << i << ": \n " << state.pose->getValue() << "\n"; + } + return 0; +} diff --git a/samples/SimpleP2PandCATrajPrior.cpp b/samples/SimpleP2PandCATrajPrior.cpp new file mode 100644 index 0000000..2a386ba --- /dev/null +++ b/samples/SimpleP2PandCATrajPrior.cpp @@ -0,0 +1,235 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SimpleP2PandCATrajPrior.cpp +/// \brief A sample usage of the STEAM Engine library for a bundle adjustment problem +/// with point-to-point error terms and trajectory smoothing factors. +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include +#include +#include +#include + +using namespace std; +typedef steam::PointToPointErrorEval Error; +typedef steam::WeightedLeastSqCostTerm<4,6> Cost; +unsigned int M = 20; +unsigned int N = 20; + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Structure to store trajectory state variables +////////////////////////////////////////////////////////////////////////////////////////////// +struct TrajStateVar { + steam::Time time; + steam::se3::TransformStateVar::Ptr pose; + steam::VectorSpaceStateVar::Ptr velocity; + steam::VectorSpaceStateVar::Ptr acceleration; +}; + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Example that loads and solves simple bundle adjustment problems +////////////////////////////////////////////////////////////////////////////////////////////// +int main(int argc, char** argv) { + + /// + /// Setup Traj Prior + /// + + + // Smoothing factor diagonal -- in this example, we penalize accelerations in each dimension + // except for the forward and yaw (this should be fairly typical) + double lin_acc_stddev_x = 1.00; // body-centric (e.g. x is usually forward) + double lin_acc_stddev_y = 0.01; // body-centric (e.g. y is usually side-slip) + double lin_acc_stddev_z = 0.01; // body-centric (e.g. z is usually 'jump') + double ang_acc_stddev_x = 0.01; // ~roll + double ang_acc_stddev_y = 0.01; // ~pitch + double ang_acc_stddev_z = 1.00; // ~yaw + Eigen::Array Qc_diag; + Qc_diag << lin_acc_stddev_x, lin_acc_stddev_y, lin_acc_stddev_z, + ang_acc_stddev_x, ang_acc_stddev_y, ang_acc_stddev_z; + Qc_diag = Qc_diag * 1; + + // Make Qc_inv + Eigen::Matrix Qc_inv; Qc_inv.setZero(); + Qc_inv.diagonal() = 1.0/Qc_diag; + + /// + /// Parse Dataset + /// + + string filePath = "../../include/steam/data/PointToPoint/points_simulated.txt"; + ifstream in(filePath); + + vector> fields; + int count = 0; + if (in) { + string line; + while (getline(in, line)) { + count++; + stringstream sep(line); + string field; + fields.push_back(vector()); + while (getline(sep, field, ',')) { + fields.back().push_back(stod(field)); + } + } + } + + filePath = "../../include/steam/data/PointToPoint/poses_ic.txt"; + ifstream inIc(filePath); + + vector> fieldsIC; + count = 0; + if (inIc) { + string line; + while (getline(inIc, line)) { + count++; + stringstream sep(line); + string field; + fieldsIC.push_back(vector()); + while (getline(sep, field, ',')) { + fieldsIC.back().push_back(stod(field)); + } + } + } + + cout << fieldsIC.size() << endl; + cout << fieldsIC.back().front() << endl; + + // State variable containers (and related data) + std::vector traj_states_ic; + + // Zero velocity + Eigen::Matrix initVelocity; initVelocity.setZero(); + // initVelocity << -10.0, 0, 0, 0, 0, 0; + + // Zero acceleration + Eigen::Matrix initAcceleration; initAcceleration.setZero(); + // initAcceleration << -3.0, 0, 0, 0, 0, 0; + + // Setup state variables using initial condition + for (unsigned int i = 0; i < N; i++) { + TrajStateVar temp; + double dt = double(i-0)*0.1; + temp.time = steam::Time(dt); + Eigen::Matrix T_k0; + T_k0 << fieldsIC[i][0], fieldsIC[i][1], fieldsIC[i][2], fieldsIC[i][3], + fieldsIC[i][4], fieldsIC[i][5], fieldsIC[i][6], fieldsIC[i][7], + fieldsIC[i][8], fieldsIC[i][9], fieldsIC[i][10], fieldsIC[i][11], + 0.0, 0.0, 0.0, 1; + + lgmath::se3::Transformation T_k0_tf(T_k0); + + temp.pose = steam::se3::TransformStateVar::Ptr(new steam::se3::TransformStateVar(T_k0_tf)); + initVelocity << fieldsIC[i+20][0],fieldsIC[i+20][1],fieldsIC[i+20][2],fieldsIC[i+20][3],fieldsIC[i+20][4],fieldsIC[i+20][5]; + initAcceleration << fieldsIC[i+20][6],fieldsIC[i+20][7],fieldsIC[i+20][8],fieldsIC[i+20][9],fieldsIC[i+20][10],fieldsIC[i+20][11]; + cout << initVelocity.transpose() << endl; + + temp.velocity = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initVelocity)); + temp.acceleration = steam::VectorSpaceStateVar::Ptr(new steam::VectorSpaceStateVar(initAcceleration)); + traj_states_ic.push_back(temp); + } + + // Setup Trajectory + steam::se3::SteamCATrajInterface traj(Qc_inv); + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + TrajStateVar& state = traj_states_ic.at(i); + steam::se3::TransformStateEvaluator::Ptr temp = + steam::se3::TransformStateEvaluator::MakeShared(state.pose); + traj.add(state.time, temp, state.velocity, state.acceleration); + } + + // Lock first pose (otherwise entire solution is 'floating') + // **Note: alternatively we could add a prior to the first pose. + traj_states_ic[0].pose->setLock(true); + + + // steam cost terms + steam::ParallelizedCostTermCollection::Ptr measCostTerms(new steam::ParallelizedCostTermCollection()); + + // Setup shared noise and loss function + Eigen::Matrix4d measurementNoise; + measurementNoise.setIdentity(); + steam::BaseNoiseModel<4>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<4>(measurementNoise)); + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + Error::Ptr error; + Cost::Ptr cost; + + // cout << fields[1*N+1][1] << endl; + + for (unsigned int i = 1; i < N; i++) { + steam::se3::TransformEvaluator::Ptr T_k0 = steam::se3::TransformStateEvaluator::MakeShared(traj_states_ic[i].pose); + + for (unsigned int j = 0; j < M; j++) { + Eigen::Vector4d ref_a; + ref_a << fields[j][0] , fields[j][1] , fields[j][2] , fields[j][3]; + + Eigen::Vector4d read_b; + read_b << fields[i*N+j][0] , fields[i*N+j][1] , fields[i*N+j][2] , fields[i*N+j][3]; + + error.reset(new Error(ref_a, read_b, T_k0)); + cost.reset(new Cost(error, sharedNoiseModel, sharedLossFunc)); + measCostTerms->add(cost); + } + } + + // Trajectory prior smoothing terms + steam::ParallelizedCostTermCollection::Ptr smoothingCostTerms(new steam::ParallelizedCostTermCollection()); + traj.appendPriorCostTerms(smoothingCostTerms); + + /// + /// Make Optimization Problem + /// + + // Initialize problem + steam::OptimizationProblem problem; + + // Add state variables + for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + const TrajStateVar& state = traj_states_ic.at(i); + problem.addStateVariable(state.pose); + problem.addStateVariable(state.velocity); + problem.addStateVariable(state.acceleration); + } + + // Add cost terms + problem.addCostTerm(measCostTerms); + problem.addCostTerm(smoothingCostTerms); + + /// + /// Setup Solver and Optimize + /// + typedef steam::VanillaGaussNewtonSolver SolverType; + + // Initialize parameters (enable verbose mode) + SolverType::Params params; + params.verbose = true; + params.absoluteCostChangeThreshold = 1e-20; + params.relativeCostChangeThreshold = 1e-20; + + // Make solver + SolverType solver(&problem, params); + + // Optimize + TrajStateVar& state = traj_states_ic.back(); + cout << state.velocity->getValue().transpose() << endl; + cout << state.acceleration->getValue().transpose() << endl; + + solver.optimize(); + + // Setup Trajectory + // for (unsigned int i = 0; i < traj_states_ic.size(); i++) { + // TrajStateVar& state = traj_states_ic.at(i); + // std::cout << i << ": \n " << state.pose->getValue() << "\n"; + // } + + state = traj_states_ic.back(); + cout << state.pose->getValue() << endl; + cout << state.velocity->getValue().transpose() << endl; + cout << state.acceleration->getValue().transpose() << endl; + return 0; + +} diff --git a/src/evaluator/blockauto/transform/ConstAccTransformEvaluator.cpp b/src/evaluator/blockauto/transform/ConstAccTransformEvaluator.cpp new file mode 100644 index 0000000..d461733 --- /dev/null +++ b/src/evaluator/blockauto/transform/ConstAccTransformEvaluator.cpp @@ -0,0 +1,142 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file ConstAccTransformEvaluator.cpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +ConstAccTransformEvaluator::ConstAccTransformEvaluator( + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Time& time) : velocity_(velocity), acceleration_(acceleration), + time_(time) { + + if(velocity->getPerturbDim() != 6) { + throw std::invalid_argument("[ConstVelTransformEval] velocity was not 6D."); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Pseudo constructor - return a shared pointer to a new instance +////////////////////////////////////////////////////////////////////////////////////////////// +ConstAccTransformEvaluator::Ptr ConstAccTransformEvaluator::MakeShared( + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Time& time) { + return ConstAccTransformEvaluator::Ptr(new ConstAccTransformEvaluator(velocity, + acceleration, time)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +bool ConstAccTransformEvaluator::isActive() const { + return !velocity_->isLocked(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Adds references (shared pointers) to active state variables to the map output +////////////////////////////////////////////////////////////////////////////////////////////// +void ConstAccTransformEvaluator::getActiveStateVariables( + std::map* outStates) const { + if (this->isActive()) { + (*outStates)[velocity_->getKey().getID()] = velocity_; + (*outStates)[acceleration_->getKey().getID()] = acceleration_; + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the transformation matrix +////////////////////////////////////////////////////////////////////////////////////////////// +lgmath::se3::Transformation ConstAccTransformEvaluator::evaluate() const { + Eigen::Matrix xi = time_.seconds() * velocity_->getValue() + + 0.5*time_.seconds()*time_.seconds()*acceleration_->getValue(); + return lgmath::se3::Transformation(xi); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the transformation matrix tree +////////////////////////////////////////////////////////////////////////////////////////////// +EvalTreeNode* ConstAccTransformEvaluator::evaluateTree() const { + + // Make new leaf node -- note we get memory from the pool + EvalTreeNode* result = EvalTreeNode::pool.getObj(); + Eigen::Matrix xi = time_.seconds() * velocity_->getValue() + + 0.5*time_.seconds()*time_.seconds()*acceleration_->getValue(); + result->setValue(lgmath::se3::Transformation(xi)); + return result; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Implementation for Block Automatic Differentiation +////////////////////////////////////////////////////////////////////////////////////////////// +template +void ConstAccTransformEvaluator::appendJacobiansImpl( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + + if (!velocity_->isLocked()) { + + // Make jacobian + Eigen::Matrix xi = time_.seconds() * velocity_->getValue() + + 0.5*time_.seconds()*time_.seconds()*acceleration_->getValue(); + + // TODO: check this is correct + Eigen::Matrix jac_vel = time_.seconds() * lgmath::se3::vec2jac(xi); + Eigen::Matrix jac_acc = 0.5*time_.seconds()*time_.seconds()*lgmath::se3::vec2jac(xi); + + // Add Jacobian + outJacobians->push_back(Jacobian(velocity_->getKey(), lhs*jac_vel)); + outJacobians->push_back(Jacobian(acceleration_->getKey(), lhs*jac_acc)); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::MatrixXd& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +void ConstAccTransformEvaluator::appendBlockAutomaticJacobians(const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +} // se3 +} // steam diff --git a/src/evaluator/samples/ImuErrorEval.cpp b/src/evaluator/samples/ImuErrorEval.cpp new file mode 100644 index 0000000..ecce872 --- /dev/null +++ b/src/evaluator/samples/ImuErrorEval.cpp @@ -0,0 +1,104 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file ImuErrorEval.cpp +/// +/// \author Sean Anderson, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include +namespace steam { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +ImuErrorEval::ImuErrorEval( + const Eigen::Matrix& measurement, + const Eigen::Matrix3d& C_body_enu, + const VectorSpaceStateVar::ConstPtr& varpi, + const VectorSpaceStateVar::ConstPtr& varpi_dot, + const VectorSpaceStateVar::ConstPtr& imu_bias, + const lgmath::se3::Transformation& T_s_v) + : meas_(measurement), C_body_enu_(C_body_enu), varpi_(varpi), varpi_dot_(varpi_dot), imu_bias_(imu_bias), adT_s_v_(T_s_v.adjoint()) { + gravity_=Eigen::Vector3d::Zero(); + gravity_(2)=9.81; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +bool ImuErrorEval::isActive() const { + return !varpi_->isLocked() || !varpi_dot_->isLocked() || !imu_bias_->isLocked(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the measurement error +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Matrix ImuErrorEval::evaluate() const { + Eigen::Matrix error; + error.head<3>() = meas_.head<3>() + adT_s_v_.bottomRightCorner<3,3>()*varpi_->getValue().tail<3>() - imu_bias_->getValue().head<3>(); + error.tail<3>() = meas_.tail<3>() + adT_s_v_.topLeftCorner<3,3>()*varpi_dot_->getValue().head<3>() + adT_s_v_.topRightCorner<3,3>()*varpi_->getValue().tail<3>() + - imu_bias_->getValue().tail<3>() - C_body_enu_*gravity_; + return error; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the measurement error and relevant Jacobians +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Matrix ImuErrorEval::evaluate( + const Eigen::Matrix& lhs, + std::vector >* jacs) const { + + // Check and initialize jacobian array + if (jacs == NULL) { + throw std::invalid_argument("Null pointer provided to return-input 'jacs' in evaluate"); + } + jacs->clear(); + + // Check that dimensions match + // if (lhs.cols() != stateVec_->getPerturbDim()) { + // throw std::runtime_error("evaluate had dimension mismatch."); + // } + + // Construct Jacobian + if(!varpi_->isLocked()) { + // Construct Jacobian Object + jacs->push_back(Jacobian<6,6>()); + Jacobian<6,6>& jacref = jacs->back(); + jacref.key = varpi_->getKey(); + Eigen::MatrixXd jacobian(6,6); + jacobian.setZero(); + jacobian.topRightCorner<3,3>()=adT_s_v_.bottomRightCorner<3,3>();//Eigen::Matrix3d::Identity(); + jacref.jac = lhs * jacobian; + } + + if(!varpi_dot_->isLocked()) { + // Construct Jacobian Object + jacs->push_back(Jacobian<6,6>()); + Jacobian<6,6>& jacref = jacs->back(); + jacref.key = varpi_dot_->getKey(); + Eigen::MatrixXd jacobian(6,6); + jacobian.setZero(); + jacobian.bottomLeftCorner<3,3>()=adT_s_v_.topLeftCorner<3,3>();//Eigen::Matrix3d::Identity(); + jacobian.bottomRightCorner<3,3>()=adT_s_v_.topRightCorner<3,3>(); + jacref.jac = lhs * jacobian; + } + + if(!imu_bias_->isLocked()) { + // Construct Jacobian Object + jacs->push_back(Jacobian<6,6>()); + Jacobian<6,6>& jacref = jacs->back(); + jacref.key = imu_bias_->getKey(); + Eigen::MatrixXd jacobian(6,6); + jacobian.setIdentity(); + jacref.jac = -lhs * jacobian; + } + + + // Return error + Eigen::Matrix error; + error.head<3>() = meas_.head<3>() + adT_s_v_.bottomRightCorner<3,3>()*varpi_->getValue().tail<3>() - imu_bias_->getValue().head<3>(); + error.tail<3>() = meas_.tail<3>() + adT_s_v_.topLeftCorner<3,3>()*varpi_dot_->getValue().head<3>() + adT_s_v_.topRightCorner<3,3>()*varpi_->getValue().tail<3>() + - imu_bias_->getValue().tail<3>() - C_body_enu_*gravity_; + return error; +} + +} // steam diff --git a/src/evaluator/samples/PointToPointErrorEval.cpp b/src/evaluator/samples/PointToPointErrorEval.cpp new file mode 100644 index 0000000..bb77c16 --- /dev/null +++ b/src/evaluator/samples/PointToPointErrorEval.cpp @@ -0,0 +1,95 @@ +// vim: ts=4:sw=4:noexpandtab +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file PointToPointErrorEval.cpp +/// +/// \author Francois Pomerleau, ASRL +/// \brief This evaluator was develop in the context of ICP (Iterative Closest Point) +/// implementation. +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +namespace steam { + + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +PointToPointErrorEval::PointToPointErrorEval( + const Eigen::Vector4d& ref_a, + const se3::TransformEvaluator::ConstPtr& T_a_world, + const Eigen::Vector4d& read_b, + const se3::TransformEvaluator::ConstPtr& T_b_world + ): ref_a_(ref_a), + T_b_a_(se3::ComposeInverseTransformEvaluator::MakeShared(T_a_world, T_b_world)), + read_b_(read_b) { +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +PointToPointErrorEval::PointToPointErrorEval( + const Eigen::Vector4d& ref_a, + const Eigen::Vector4d& read_b, + const se3::TransformEvaluator::ConstPtr& T_b_a + ): ref_a_(ref_a), + T_b_a_(T_b_a), + read_b_(read_b){ +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +bool PointToPointErrorEval::isActive() const { + return T_b_a_->isActive(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the 4-d measurement error (ul vl ur vr) +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Vector4d PointToPointErrorEval::evaluate() const { + + // Return error (between measurement and point estimate projected in camera frame) + // return ref_a_ - (T_b_a_->evaluate().inverse() * read_b_); + return read_b_ - T_b_a_->evaluate() * ref_a_; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the 4-d measurement error (ul vl ur vr) and Jacobians +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::Vector4d PointToPointErrorEval::evaluate(const Eigen::Matrix4d& lhs, std::vector >* jacs) const { + + // Check and initialize jacobian array + if (jacs == NULL) { + throw std::invalid_argument("Null pointer provided to return-input 'jacs' in evaluate"); + } + jacs->clear(); + + // Get evaluation tree + + EvalTreeHandle blkAutoTransform = + T_b_a_->getBlockAutomaticEvaluation(); + + // Get evaluation from tree + //TODO: why just not using T_a_b_->evaluate() instead? + const lgmath::se3::Transformation T_ba = blkAutoTransform.getValue(); + // const lgmath::se3::Transformation T_ba = T_b_a_->evaluate(); + // const Eigen::Vector4d read_a = T_ba.inverse() * read_b_; + const Eigen::Vector4d ref_b = T_ba * ref_a_; + + // Get Jacobians + + // Eigen::Matrix newLhs = -lhs*lgmath::se3::point2fs(read_a.head<3>()); + // Eigen::Matrix newLhs = lhs*T_ba.adjoint()*lgmath::se3::point2fs(read_a.head<3>()); + Eigen::Matrix newLhs = -lhs*lgmath::se3::point2fs(ref_b.head<3>()); + + T_b_a_->appendBlockAutomaticJacobians(newLhs, blkAutoTransform.getRoot(), jacs); + + // Return evaluation + // return ref_a_ - read_a; + return read_b_ - ref_b; +} + + +} // steam diff --git a/src/solver/GaussNewtonSolverBase.cpp b/src/solver/GaussNewtonSolverBase.cpp index 8bf9e82..b093e52 100644 --- a/src/solver/GaussNewtonSolverBase.cpp +++ b/src/solver/GaussNewtonSolverBase.cpp @@ -153,6 +153,10 @@ void GaussNewtonSolverBase::factorizeHessian(const Eigen::SparseMatrix& // Check if the factorization succeeded if (hessianSolver_.info() != Eigen::Success) { + + std::cout << "Approximate Hessian is: " << std::endl; + std::cout << approximateHessian << std::endl; + throw decomp_failure("During steam solve, Eigen LLT decomposition failed. " "It is possible that the matrix was ill-conditioned, in which case " "adding a prior may help. On the other hand, it is also possible that " diff --git a/src/trajectory/SteamCATrajInterface.cpp b/src/trajectory/SteamCATrajInterface.cpp new file mode 100644 index 0000000..abcc53b --- /dev/null +++ b/src/trajectory/SteamCATrajInterface.cpp @@ -0,0 +1,484 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajInterface.cpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include + +#include +#include +#include + +#include +#include +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Add a new knot +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajInterface::add(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration) { + + // Check velocity input + if (velocity->getPerturbDim() != 6) { + throw std::invalid_argument("invalid velocity size"); + } + + // Check acceleration input + if (acceleration->getPerturbDim() != 6) { + throw std::invalid_argument("invalid acceleration size"); + } + + // Todo, check that time does not already exist in map? + + // Make knot + SteamCATrajVar::Ptr newEntry(new SteamCATrajVar(time, T_k0, velocity, acceleration)); + + // Insert in map + knotMap_.insert(knotMap_.end(), + std::pair(time.nanosecs(), newEntry)); +} + +void SteamCATrajInterface::add(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov) { + + // Check velocity input + if (velocity->getPerturbDim() != 6) { + throw std::invalid_argument("invalid velocity size"); + } + + // Check acceleration input + if (acceleration->getPerturbDim() != 6) { + throw std::invalid_argument("invalid acceleration size"); + } + + // Todo, check that time does not already exist in map? + + // Make knot + SteamCATrajVar::Ptr newEntry(new SteamCATrajVar(time, T_k0, velocity, acceleration, cov)); + + // Insert in map + knotMap_.insert(knotMap_.end(), + std::pair(time.nanosecs(), newEntry)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get evaluator +////////////////////////////////////////////////////////////////////////////////////////////// +TransformEvaluator::ConstPtr SteamCATrajInterface::getInterpPoseEval(const steam::Time& time) const { + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); + } + + // Get iterator to first element with time equal to or great than 'time' + std::map::const_iterator it1 + = knotMap_.lower_bound(time.nanosecs()); + + // Check if time is passed the last entry + if (it1 == knotMap_.end()) { + + // If we allow extrapolation, return constant-acceleration interpolated entry + if (allowExtrapolation_) { + --it1; // should be safe, as we checked that the map was not empty.. + const SteamTrajVar::Ptr& endKnot = it1->second; + TransformEvaluator::Ptr T_t_k = + ConstAccTransformEvaluator::MakeShared(endKnot->getVelocity(), + endKnot->getAcceleration(), + time - endKnot->getTime()); + return compose(T_t_k, endKnot->getPose()); + } else { + throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + } + } + + // Check if we requested time exactly + if (it1->second->getTime() == time) { + + // return state variable exactly (no interp) + return it1->second->getPose(); + } + + // Check if we requested before first time + if (it1 == knotMap_.begin()) { + + // If we allow extrapolation, return constant-acceleration interpolated entry + if (allowExtrapolation_) { + const SteamTrajVar::Ptr& startKnot = it1->second; + TransformEvaluator::Ptr T_t_k = + ConstAccTransformEvaluator::MakeShared(startKnot->getVelocity(), + startKnot->getAcceleration(), + time - startKnot->getTime()); + return compose(T_t_k, startKnot->getPose()); + } else { + throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + } + } + + // Get iterators bounding the time interval + std::map::const_iterator it2 = it1; --it1; + if (time <= it1->second->getTime() || time >= it2->second->getTime()) { + throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " + "should not trigger... report to a STEAM contributor."); + } + + // Create interpolated evaluator + return SteamCATrajPoseInterpEval::MakeShared(time, it1->second, it2->second); +} + +Eigen::VectorXd SteamCATrajInterface::getVelocity(const steam::Time& time) { + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); + } + + // Get iterator to first element with time equal to or greater than 'time' + std::map::const_iterator it1 + = knotMap_.lower_bound(time.nanosecs()); + + // Check if time is passed the last entry + if (it1 == knotMap_.end()) { + + // If we allow extrapolation, return constant-velocity interpolated entry + if (allowExtrapolation_) { + --it1; // should be safe, as we checked that the map was not empty.. + + const SteamTrajVar::Ptr& endKnot = it1->second; + + auto dt=(time-endKnot->getTime()).seconds(); + auto xi_tk=endKnot->getVelocity()->getValue()*dt+0.5*dt*dt*endKnot->getAcceleration()->getValue(); + + Eigen::Matrix J_tk = lgmath::se3::vec2jac(xi_tk); + return J_tk*(endKnot->getVelocity()->getValue()+endKnot->getAcceleration()->getValue()*dt); + } else { + throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + } + } + + // Check if we requested time exactly + if (it1->second->getTime() == time) { + const SteamTrajVar::Ptr& knot = it1->second; + // return state variable exactly (no interp) + return knot->getVelocity()->getValue(); + } + + // Check if we requested before first time + if (it1 == knotMap_.begin()) { + // If we allow extrapolation, return constant-velocity interpolated entry + if (allowExtrapolation_) { + const SteamTrajVar::Ptr& startKnot = it1->second; + return startKnot->getVelocity()->getValue(); + } else { + throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + } + } + + // Get iterators bounding the time interval + std::map::const_iterator it2 = it1; --it1; + if (time <= it1->second->getTime() || time >= it2->second->getTime()) { + throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " + "should not trigger... report to a STEAM contributor."); + } + + // OK, we actually need to interpolate. + // Follow a similar setup to SteamCATrajPoseInterpEval + + // Convenience defs + auto &knot1 = it1->second; + auto &knot2 = it2->second; + + double t1 = knot1->getTime().seconds(); + double t2 = knot2->getTime().seconds(); + double tau = time.seconds(); + + // Cheat by calculating deltas wrt t1, so we can avoid super large values + tau = tau-t1; + t2 = t2-t1; + t1 = 0; + + double T = (knot2->getTime() - knot1->getTime()).seconds(); + double delta_tau = (time - knot1->getTime()).seconds(); + double delta_kappa = (knot2->getTime()-time).seconds(); + + // std::cout << t1 << " " << t2 << " " << tau << std::endl; + + double T2 = T*T; + double T3 = T2*T; + double T4 = T3*T; + double T5 = T4*T; + + double delta_tau2 = delta_tau*delta_tau; + double delta_tau3 = delta_tau2*delta_tau; + double delta_tau4 = delta_tau3*delta_tau; + double delta_kappa2 = delta_kappa*delta_kappa; + double delta_kappa3 = delta_kappa2*delta_kappa; + + // Calculate 'omega' interpolation values + double omega11 = delta_tau3/T5*(t1*t1 - 5*t1*t2 + 3*t1*tau + 10*t2*t2 - 15*t2*tau + 6*tau*tau); + double omega12 = delta_tau3*delta_kappa/T4*(t1 - 4*t2 + 3*tau); + double omega13 = delta_tau3*delta_kappa2/(2*T3); + + double omega21 = 30*delta_tau2/T5*(6*delta_kappa2-4*delta_tau*T+8*delta_tau*delta_kappa-6*T*delta_kappa+3*delta_tau2+T2); + double omega22 = -delta_tau2/T4*(90*delta_kappa2-64*delta_tau*T+120*delta_tau*delta_kappa-96*T*delta_kappa+45*delta_tau2+18*T2); + double omega23 = delta_tau2/(2*T3)*(30*delta_kappa2-24*delta_tau*T+40*delta_tau*delta_kappa-36*T*delta_kappa+15*delta_tau2+9*T2); + + // Calculate 'lambda' interpolation values + double lambda12 = delta_tau*delta_kappa3/T4*(t2 - 4*t1 + 3*tau); + double lambda13 = delta_tau2*delta_kappa3/(2*T3); + + double lambda22 = -(90*delta_tau2*delta_kappa2-56*delta_tau3*T+45*delta_tau4-T4+120*delta_tau3*delta_kappa+12*delta_tau2*T2-84*delta_tau2*T*delta_kappa)/T4; + double lambda23 = -delta_tau/(2*T3)*(3*delta_tau*T2-16*delta_tau2*T+15*delta_tau3-2*T3+30*delta_tau*delta_kappa2+40*delta_tau2*delta_kappa-24*delta_tau*T*delta_kappa); + + // Get relative matrix info + lgmath::se3::Transformation T_21 = knot2->getPose()->evaluate()/knot1->getPose()->evaluate(); + + // Get se3 algebra of relative matrix + Eigen::Matrix xi_21 = T_21.vec(); + + // Calculate the 6x6 associated Jacobian + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + + // Intermediate variable + Eigen::Matrix varpicurl2 = lgmath::se3::curlyhat(J_21_inv*knot2->getVelocity()->getValue()); + + // Calculate interpolated relative se3 algebra + Eigen::Matrix xi_i1 = lambda12*knot1->getVelocity()->getValue() + + lambda13*knot1->getAcceleration()->getValue() + + omega11*xi_21 + + omega12*J_21_inv*knot2->getVelocity()->getValue()+ + omega13*(-0.5*varpicurl2*knot2->getVelocity()->getValue() + J_21_inv*knot2->getAcceleration()->getValue()); + + // Calculate the 6x6 associated Jacobian + Eigen::Matrix J_t1 = lgmath::se3::vec2jac(xi_i1); + + // Calculate interpolated relative se3 algebra + Eigen::VectorXd xi_it = J_t1*(lambda22*knot1->getVelocity()->getValue() + + lambda23*knot1->getAcceleration()->getValue() + + omega21*xi_21 + + omega22*J_21_inv*knot2->getVelocity()->getValue() + + omega23*(-0.5*varpicurl2*knot2->getVelocity()->getValue() + J_21_inv*knot2->getAcceleration()->getValue())); + + return xi_it; + } + + Eigen::VectorXd SteamCATrajInterface::getAcceleration(const steam::Time& time) { + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); + } + + // Get iterator to first element with time equal to or greater than 'time' + std::map::const_iterator it1 + = knotMap_.lower_bound(time.nanosecs()); + + // Check if time is passed the last entry + if (it1 == knotMap_.end()) { + + // If we allow extrapolation, return constant-velocity interpolated entry + if (allowExtrapolation_) { + --it1; // should be safe, as we checked that the map was not empty.. + const SteamTrajVar::Ptr& endKnot = it1->second; + return endKnot->getAcceleration()->getValue(); + } else { + throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + } + } + + // Check if we requested time exactly + // if (it1->second->getTime() == time) { + const SteamTrajVar::Ptr& knot = it1->second; + // return state variable exactly (no interp) + return knot->getAcceleration()->getValue(); + // } + + // // Check if we requested before first time + // if (it1 == knotMap_.begin()) { + // // If we allow extrapolation, return constant-velocity interpolated entry + // if (allowExtrapolation_) { + // const SteamTrajVar::Ptr& startKnot = it1->second; + // return startKnot->getAcceleration()->getValue(); + // } else { + // throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + // } + // } + } + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Add a unary acceleration prior factor at a knot time. Note that only a single acceleration +/// prior should exist on a trajectory, adding a second will overwrite the first. +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajInterface::addAccelerationPrior(const steam::Time& time, + const Eigen::Matrix& acceleration, + const Eigen::Matrix& cov) { + + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][addVelocityPrior] map was empty."); + } + + // Try to find knot at same time + std::map::const_iterator it = knotMap_.find(time.nanosecs()); + if (it == knotMap_.end()) { + throw std::runtime_error("[GpTrajectory][addVelocityPrior] no knot at provided time."); + } + + // Get reference + const SteamTrajVar::Ptr& knotRef = it->second; + + // Check that the pose is not locked + if(knotRef->getAcceleration()->isLocked()) { + throw std::runtime_error("[GpTrajectory][addAccelerationPrior] tried to add prior to locked acceleration."); + } + + // Set up loss function, noise model, and error function + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + steam::BaseNoiseModel<6>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<6>(cov)); + steam::VectorSpaceErrorEval<6,6>::Ptr errorfunc(new steam::VectorSpaceErrorEval<6,6>(acceleration, knotRef->getAcceleration())); + + // Create cost term + accelerationPriorFactor_ = steam::WeightedLeastSqCostTerm<6,6>::Ptr( + new steam::WeightedLeastSqCostTerm<6,6>(errorfunc, sharedNoiseModel, sharedLossFunc)); +} + + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get cost terms associated with the prior for unlocked parts of the trajectory +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajInterface::appendPriorCostTerms( + const ParallelizedCostTermCollection::Ptr& costTerms) const { + + // If empty, return none + if (knotMap_.empty()) { + return; + } + + // Check for pose, velocity priors, and acceleration priors + if (posePriorFactor_) { + costTerms->add(posePriorFactor_); + } + if (velocityPriorFactor_) { + costTerms->add(velocityPriorFactor_); + } + if (accelerationPriorFactor_) { + costTerms->add(accelerationPriorFactor_); + } + // All prior factors will use an L2 loss function + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + // steam::GemanMcClureLossFunc::Ptr sharedLossFunc(new steam::GemanMcClureLossFunc(1)); + + // Initialize first iterator + std::map::const_iterator it1 = knotMap_.begin(); + if (it1 == knotMap_.end()) { + throw std::runtime_error("No knots..."); + } + + // Iterate through all states.. if any are unlocked, supply a prior term + std::map::const_iterator it2 = it1; ++it2; + for (; it2 != knotMap_.end(); ++it1, ++it2) { + + // Get knots + const SteamTrajVar::ConstPtr& knot1 = it1->second; + const SteamTrajVar::ConstPtr& knot2 = it2->second; + + // Check if any of the variables are unlocked + if(knot1->getPose()->isActive() || !knot1->getVelocity()->isLocked() || + knot2->getPose()->isActive() || !knot2->getVelocity()->isLocked() || + !knot2->getAcceleration()->isLocked() || !knot2->getAcceleration()->isLocked()) { + + // Generate 18 x 18 information matrix for GP prior factor + Eigen::Matrix Qi_inv; + double one_over_dt = 1.0/(knot2->getTime() - knot1->getTime()).seconds(); + double one_over_dt2 = one_over_dt*one_over_dt; + double one_over_dt3 = one_over_dt2*one_over_dt; + double one_over_dt4 = one_over_dt3*one_over_dt; + double one_over_dt5 = one_over_dt4*one_over_dt; + + Qi_inv.block<6,6>(0,0) = 720.0 * one_over_dt5 * Qc_inv_; + Qi_inv.block<6,6>(6,6) = 192.0 * one_over_dt3 * Qc_inv_; + Qi_inv.block<6,6>(12,12) = 9.0 * one_over_dt * Qc_inv_; + Qi_inv.block<6,6>(6,0) = Qi_inv.block<6,6>(0,6) = -360.0 * one_over_dt4 * Qc_inv_; + Qi_inv.block<6,6>(12,0) = Qi_inv.block<6,6>(0,12) = 60.0 * one_over_dt3 * Qc_inv_; + Qi_inv.block<6,6>(12,6) = Qi_inv.block<6,6>(6,12) = -36.0 * one_over_dt2 * Qc_inv_; + + // std::cout << Qi_inv.block<6,6>(12,0) << std::endl << std::endl; + // std::cout << Qi_inv.block<6,6>(0,12) << std::endl; + steam::BaseNoiseModelX::Ptr sharedGPNoiseModel( + new steam::StaticNoiseModelX(Qi_inv, steam::INFORMATION)); + + // Create cost term + steam::se3::SteamCATrajPriorFactor::Ptr errorfunc( + new steam::se3::SteamCATrajPriorFactor(knot1, knot2)); + steam::WeightedLeastSqCostTermX::Ptr cost( + new steam::WeightedLeastSqCostTermX(errorfunc, sharedGPNoiseModel, sharedLossFunc)); + costTerms->add(cost); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get interpolated/extrapolated covariance at given time, for now only support at knot times +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::MatrixXd SteamCATrajInterface::getCovariance(const steam::Time& time) const { + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); + } + + // Get iterator to first element with time equal to or great than 'time' + std::map::const_iterator it1 + = knotMap_.lower_bound(time.nanosecs()); + + // Check if time is passed the last entry + if (it1 == knotMap_.end()) { + --it1; // last knot + // If we allow extrapolation + // if (allowExtrapolation_) { + // return extrapCovariance(solver, time); + // } else { + // throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + // } + } + + // Check if we requested time exactly + // if (it1->second->getTime() == time) { + // return covariance exactly (no interp) + if (it1->second->covarianceSet()) { + return it1->second->getCovariance(); + } + std::map outState; + it1->second->getPose()->getActiveStateVariables(&outState); + + std::vector keys; + keys.push_back(outState.begin()->second->getKey()); + keys.push_back(it1->second->getVelocity()->getKey()); + keys.push_back(it1->second->getAcceleration()->getKey()); + + steam::BlockMatrix covariance = solver_->queryCovarianceBlock(keys); + + Eigen::Matrix output; + output.block<6,6>(0,0) = covariance.copyAt(0,0); + output.block<6,6>(0,6) = covariance.copyAt(0,1); + output.block<6,6>(0,12) = covariance.copyAt(0,2); + output.block<6,6>(6,0) = covariance.copyAt(1,0); + output.block<6,6>(6,6) = covariance.copyAt(1,1); + output.block<6,6>(6,12) = covariance.copyAt(1,2); + output.block<6,6>(12,0) = covariance.copyAt(2,0); + output.block<6,6>(12,6) = covariance.copyAt(2,1); + output.block<6,6>(12,12) = covariance.copyAt(2,2); + return output; + // } +} + +} // se3 +} // steam diff --git a/src/trajectory/SteamCATrajPoseInterpEval.cpp b/src/trajectory/SteamCATrajPoseInterpEval.cpp new file mode 100644 index 0000000..40f160c --- /dev/null +++ b/src/trajectory/SteamCATrajPoseInterpEval.cpp @@ -0,0 +1,343 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajPoseInterpEval.cpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +SteamCATrajPoseInterpEval::SteamCATrajPoseInterpEval(const Time& time, + const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2) : + knot1_(knot1), knot2_(knot2) { + + // Calculate time constants + double t1 = knot1->getTime().seconds(); + double t2 = knot2->getTime().seconds(); + double tau = time.seconds(); + + // Cheat by calculating deltas wrt t1, so we can avoid super large values + tau = tau-t1; + t2 = t2-t1; + t1 = 0; + + double T = (knot2->getTime() - knot1->getTime()).seconds(); + double delta_tau = (time - knot1->getTime()).seconds(); + double delta_kappa = (knot2->getTime()-time).seconds(); + + // std::cout << t1 << " " << t2 << " " << tau << std::endl; + + double T2 = T*T; + double T3 = T2*T; + double T4 = T3*T; + double T5 = T4*T; + + double delta_tau2 = delta_tau*delta_tau; + double delta_tau3 = delta_tau2*delta_tau; + double delta_kappa2 = delta_kappa*delta_kappa; + double delta_kappa3 = delta_kappa2*delta_kappa; + + // Calculate 'omega' interpolation values + omega11_ = delta_tau3/T5*(t1*t1 - 5*t1*t2 + 3*t1*tau + 10*t2*t2 - 15*t2*tau + 6*tau*tau); + omega12_ = delta_tau3*delta_kappa/T4*(t1 - 4*t2 + 3*tau); + omega13_ = delta_tau3*delta_kappa2/(2*T3); + + // Calculate 'lambda' interpolation values + lambda12_ = delta_tau*delta_kappa3/T4*(t2 - 4*t1 + 3*tau); + lambda13_ = delta_tau2*delta_kappa3/(2*T3); + + // std::cout << "CA interpolation!" << std::endl; + // std::cout << "omega11_" << omega11_ << std::endl; + // std::cout << "ratio: " << t1*t1 - 5*t1*t2 + 3*t1*tau + 10*t2*t2 - 15*t2*tau + 6*tau*tau << std::endl; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Pseudo constructor - return a shared pointer to a new instance +////////////////////////////////////////////////////////////////////////////////////////////// +SteamCATrajPoseInterpEval::Ptr SteamCATrajPoseInterpEval::MakeShared(const Time& time, + const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2) { + return SteamCATrajPoseInterpEval::Ptr(new SteamCATrajPoseInterpEval(time, knot1, knot2)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +bool SteamCATrajPoseInterpEval::isActive() const { + return knot1_->getPose()->isActive() || + !knot1_->getVelocity()->isLocked() || + knot2_->getPose()->isActive() || + !knot2_->getVelocity()->isLocked(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Adds references (shared pointers) to active state variables to the map output +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::getActiveStateVariables( + std::map* outStates) const { + + knot1_->getPose()->getActiveStateVariables(outStates); + knot2_->getPose()->getActiveStateVariables(outStates); + if (!knot1_->getVelocity()->isLocked()) { + (*outStates)[knot1_->getVelocity()->getKey().getID()] = knot1_->getVelocity(); + } + if (!knot2_->getVelocity()->isLocked()) { + (*outStates)[knot2_->getVelocity()->getKey().getID()] = knot2_->getVelocity(); + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the transformation matrix +////////////////////////////////////////////////////////////////////////////////////////////// +lgmath::se3::Transformation SteamCATrajPoseInterpEval::evaluate() const { + + // Get relative matrix info + lgmath::se3::Transformation T_21 = knot2_->getPose()->evaluate()/knot1_->getPose()->evaluate(); + + // Get se3 algebra of relative matrix + Eigen::Matrix xi_21 = T_21.vec(); + + // Calculate the 6x6 associated Jacobian + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + + // Intermediate variable + Eigen::Matrix varpicurl2 = lgmath::se3::curlyhat(J_21_inv*knot2_->getVelocity()->getValue()); + + // Calculate interpolated relative se3 algebra + Eigen::Matrix xi_i1 = lambda12_*knot1_->getVelocity()->getValue() + + lambda13_*knot1_->getAcceleration()->getValue() + + omega11_*xi_21 + + omega12_*J_21_inv*knot2_->getVelocity()->getValue()+ + omega13_*(-0.5*varpicurl2*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue()); + + // Calculate interpolated relative transformation matrix + lgmath::se3::Transformation T_i1(xi_i1); + + // Return `global' interpolated transform + // std::cout << "Interpolated xi_i1: " << std::endl; + // std::cout << xi_i1.transpose() << std::endl; + + // std::cout << "part1: " << lambda12_*knot1_->getVelocity()->getValue().transpose() << std::endl; + // std::cout << "part2: " << lambda13_*knot1_->getAcceleration()->getValue().transpose() << std::endl; + // std::cout << "part3: " << omega11_*xi_21.transpose() << std::endl; + // std::cout << "part4: " << (omega12_*J_21_inv*knot2_->getVelocity()->getValue()).transpose() << std::endl; + // std::cout << "part5: " << (omega13_*(-0.5*varpicurl2*knot2_->getVelocity()->getValue())).transpose() << std::endl; + // std::cout << "part6: " << (omega13_*J_21_inv*knot2_->getAcceleration()->getValue()).transpose() << std::endl; + return T_i1*knot1_->getPose()->evaluate(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the transformation matrix tree +////////////////////////////////////////////////////////////////////////////////////////////// +EvalTreeNode* SteamCATrajPoseInterpEval::evaluateTree() const { + + // Evaluate sub-trees + EvalTreeNode* transform1 = knot1_->getPose()->evaluateTree(); + EvalTreeNode* transform2 = knot2_->getPose()->evaluateTree(); + + // Get relative matrix info + lgmath::se3::Transformation T_21 = transform2->getValue()/transform1->getValue(); + + // Get se3 algebra of relative matrix + Eigen::Matrix xi_21 = T_21.vec(); + + // Calculate the 6x6 associated Jacobian + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + + // Intermediate variable + Eigen::Matrix varpicurl2 = lgmath::se3::curlyhat(J_21_inv*knot2_->getVelocity()->getValue()); + + // Calculate interpolated relative se3 algebra + Eigen::Matrix xi_i1 = lambda12_*knot1_->getVelocity()->getValue() + + lambda13_*knot1_->getAcceleration()->getValue() + + omega11_*xi_21 + + omega12_*J_21_inv*knot2_->getVelocity()->getValue()+ + omega13_*(-0.5*varpicurl2*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue()); + + // Calculate interpolated relative transformation matrix + lgmath::se3::Transformation T_i1(xi_i1); + + // Interpolated relative transform - new root node (using pool memory) + EvalTreeNode* root = EvalTreeNode::pool.getObj(); + root->setValue(T_i1*transform1->getValue()); + + // Add children + root->addChild(transform1); + root->addChild(transform2); + + // Return new root node + return root; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Implementation for Block Automatic Differentiation +////////////////////////////////////////////////////////////////////////////////////////////// +template +void SteamCATrajPoseInterpEval::appendJacobiansImpl( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + + // Cast back to transformations + EvalTreeNode* transform1 = + static_cast*>(evaluationTree->childAt(0)); + EvalTreeNode* transform2 = + static_cast*>(evaluationTree->childAt(1)); + + // Get relative matrix info + lgmath::se3::Transformation T_21 = transform2->getValue()/transform1->getValue(); + + // Get se3 algebra of relative matrix + Eigen::Matrix xi_21 = T_21.vec(); + + // Calculate the 6x6 associated Jacobian + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + + // Intermediate variable + Eigen::Matrix varpicurl2 = lgmath::se3::curlyhat(J_21_inv*knot2_->getVelocity()->getValue()); + Eigen::Matrix varpicurl = lgmath::se3::curlyhat(knot2_->getVelocity()->getValue()); + + // Calculate interpolated relative se3 algebra + Eigen::Matrix xi_i1 = lambda12_*knot1_->getVelocity()->getValue() + + lambda13_*knot1_->getAcceleration()->getValue() + + omega11_*xi_21 + + omega12_*J_21_inv*knot2_->getVelocity()->getValue()+ + omega13_*(-0.5*varpicurl2*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue()); + + // Calculate interpolated relative transformation matrix + lgmath::se3::Transformation T_i1(xi_i1); + + // Calculate the 6x6 Jacobian associated with the interpolated relative transformation matrix + Eigen::Matrix J_i1 = lgmath::se3::vec2jac(xi_i1); + + // Check if evaluator is active + if (this->isActive()) { + + // Pose Jacobians + if (knot1_->getPose()->isActive() || knot2_->getPose()->isActive()) { + + // Precompute matrix + Eigen::Matrix w = omega11_*J_i1*J_21_inv + + 0.5*omega12_*J_i1*varpicurl*J_21_inv + 0.25*omega13_*J_i1*varpicurl*varpicurl*J_21_inv+ + 0.5*omega13_*J_i1*lgmath::se3::curlyhat(knot2_->getAcceleration()->getValue())*J_21_inv; + + // Check if transform1 is active + if (knot1_->getPose()->isActive()) { + Eigen::Matrix jacobian = (-1) * w * T_21.adjoint() + T_i1.adjoint(); + knot1_->getPose()->appendBlockAutomaticJacobians(lhs*jacobian, transform1, outJacobians); + } + + // Get index of split between left and right-hand-side of Jacobians + unsigned int hintIndex = outJacobians->size(); + + // Check if transform2 is active + if (knot2_->getPose()->isActive()) { + knot2_->getPose()->appendBlockAutomaticJacobians(lhs*w, transform2, outJacobians); + } + + // Merge jacobians + Jacobian::merge(outJacobians, hintIndex); + } + + // 6 x 6 Velocity Jacobian 1 + if(!knot1_->getVelocity()->isLocked()) { + + // Add Jacobian + outJacobians->push_back(Jacobian(knot1_->getVelocity()->getKey(), lhs*lambda12_*J_i1)); + } + + // 6 x 6 Velocity Jacobian 2 + if(!knot2_->getVelocity()->isLocked()) { + + // Add Jacobian + Eigen::Matrix jacobian = omega12_*J_i1*J_21_inv - 0.5*omega13_*J_i1*(varpicurl2 - varpicurl*J_21_inv); + outJacobians->push_back(Jacobian(knot2_->getVelocity()->getKey(), lhs*jacobian)); + } + + // 6 x 6 Acceleration Jacobian 1 + if(!knot1_->getAcceleration()->isLocked()) { + + // Add Jacobian + outJacobians->push_back(Jacobian(knot1_->getAcceleration()->getKey(), lhs*lambda13_*J_i1)); + } + + // 6 x 6 Acceleration Jacobian 2 + if(!knot2_->getAcceleration()->isLocked()) { + + // Add Jacobian + Eigen::Matrix jacobian = omega13_*J_i1*J_21_inv; + outJacobians->push_back(Jacobian(knot2_->getAcceleration()->getKey(), lhs*jacobian)); + } + } +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::MatrixXd& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Fixed-size evaluations of the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Fixed-size evaluations of the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Fixed-size evaluations of the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Fixed-size evaluations of the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Fixed-size evaluations of the Jacobian tree +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamCATrajPoseInterpEval::appendBlockAutomaticJacobians( + const Eigen::Matrix& lhs, + EvalTreeNode* evaluationTree, + std::vector >* outJacobians) const { + this->appendJacobiansImpl(lhs,evaluationTree, outJacobians); +} + +} // se3 +} // steam diff --git a/src/trajectory/SteamCATrajPriorFactor.cpp b/src/trajectory/SteamCATrajPriorFactor.cpp new file mode 100644 index 0000000..49f77ea --- /dev/null +++ b/src/trajectory/SteamCATrajPriorFactor.cpp @@ -0,0 +1,192 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajPriorFactor.cpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +SteamCATrajPriorFactor::SteamCATrajPriorFactor(const SteamTrajVar::ConstPtr& knot1, + const SteamTrajVar::ConstPtr& knot2) : + knot1_(knot1), knot2_(knot2) { +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Returns whether or not an evaluator contains unlocked state variables +////////////////////////////////////////////////////////////////////////////////////////////// +bool SteamCATrajPriorFactor::isActive() const { + return knot1_->getPose()->isActive() || !knot1_->getVelocity()->isLocked() || + knot2_->getPose()->isActive() || !knot2_->getVelocity()->isLocked(); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the GP prior factor +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::VectorXd SteamCATrajPriorFactor::evaluate() const { + + // Precompute values + lgmath::se3::Transformation T_21 = knot2_->getPose()->evaluate()/knot1_->getPose()->evaluate(); + Eigen::Matrix xi_21 = T_21.vec(); + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + Eigen::Matrix w = lgmath::se3::curlyhat(J_21_inv*knot2_->getVelocity()->getValue()); + + // Compute error + Eigen::Matrix error; + double dt = (knot2_->getTime() - knot1_->getTime()).seconds(); + double dt2 = dt*dt; + error.head<6>() = xi_21 - dt*knot1_->getVelocity()->getValue() - 0.5*dt2*knot1_->getAcceleration()->getValue(); + error.segment<6>(6) = J_21_inv*knot2_->getVelocity()->getValue() - knot1_->getVelocity()->getValue() - dt*knot1_->getAcceleration()->getValue(); + error.tail<6>() = -0.5*w*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue() - knot1_->getAcceleration()->getValue(); + return error; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Evaluate the GP prior factor and Jacobians +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::VectorXd SteamCATrajPriorFactor::evaluate(const Eigen::MatrixXd& lhs, + std::vector >* jacs) const { + + // Check and initialize jacobian array + if (jacs == NULL) { + throw std::invalid_argument("Null pointer provided to return-input 'jacs' in evaluate"); + } + jacs->clear(); + + // Get evaluation trees + EvalTreeNode* evaluationTree1 = knot1_->getPose()->evaluateTree(); + EvalTreeNode* evaluationTree2 = knot2_->getPose()->evaluateTree(); + + // Compute intermediate values + lgmath::se3::Transformation T_21 = evaluationTree2->getValue()/evaluationTree1->getValue(); + Eigen::Matrix xi_21 = T_21.vec(); + Eigen::Matrix J_21_inv = lgmath::se3::vec2jacinv(xi_21); + Eigen::Matrix w = lgmath::se3::curlyhat(J_21_inv*knot2_->getVelocity()->getValue()); + Eigen::Matrix v2curl = lgmath::se3::curlyhat(knot2_->getVelocity()->getValue()); + Eigen::Matrix a2curl = lgmath::se3::curlyhat(knot2_->getAcceleration()->getValue()); + + double deltaTime = (knot2_->getTime() - knot1_->getTime()).seconds(); + double dt2 = deltaTime*deltaTime; + + // Knot 1 transform + if(knot1_->getPose()->isActive()) { + Eigen::Matrix Jinv_12 = J_21_inv*T_21.adjoint(); + + // Construct jacobian + Eigen::Matrix jacobian; + jacobian.topRows<6>() = -Jinv_12; + jacobian.block<6,6>(6,0) = -0.5*v2curl*Jinv_12; + jacobian.bottomRows<6>() = -0.25*v2curl*v2curl*Jinv_12 - 0.5*a2curl*Jinv_12; + + // Get Jacobians + knot1_->getPose()->appendBlockAutomaticJacobians(lhs * jacobian, evaluationTree1, jacs); + } + + // Get index of split between left and right-hand-side of pose Jacobians + unsigned int hintIndex = jacs->size(); + + // Knot 2 transform + if(knot2_->getPose()->isActive()) { + + // Construct jacobian + Eigen::Matrix jacobian; + jacobian.topRows<6>() = J_21_inv; + jacobian.block<6,6>(6,0) = 0.5*v2curl*J_21_inv; + jacobian.bottomRows<6>() = 0.25*v2curl*v2curl*J_21_inv + 0.5*a2curl*J_21_inv; + + // Get Jacobians + knot2_->getPose()->appendBlockAutomaticJacobians(lhs * jacobian, evaluationTree2, jacs); + } + + // Merge jacobians (transform evaluators from knots 1 and 2 could contain the same + // state variables, in which case we need to merge) + Jacobian<>::merge(jacs, hintIndex); + + // Knot 1 velocity + if(!knot1_->getVelocity()->isLocked()) { + + // Construct Jacobian Object + jacs->push_back(Jacobian<>()); + Jacobian<>& jacref = jacs->back(); + jacref.key = knot1_->getVelocity()->getKey(); + + // Fill in matrix + Eigen::Matrix jacobian; + jacobian.topRows<6>() = -deltaTime*Eigen::Matrix::Identity(); + jacobian.block<6,6>(6,0) = -Eigen::Matrix::Identity(); + jacobian.bottomRows<6>() = Eigen::Matrix::Zero(); + jacref.jac = lhs * jacobian; + } + + // Knot 2 velocity + if(!knot2_->getVelocity()->isLocked()) { + + // Construct Jacobian Object + jacs->push_back(Jacobian<>()); + Jacobian<>& jacref = jacs->back(); + jacref.key = knot2_->getVelocity()->getKey(); + + // Fill in matrix + Eigen::Matrix jacobian; + jacobian.topRows<6>() = Eigen::Matrix::Zero(); + jacobian.block<6,6>(6,0) = J_21_inv; + jacobian.bottomRows<6>() = -0.5*w + 0.5*v2curl*J_21_inv; + jacref.jac = lhs * jacobian; + } + + // Knot 1 acceleration + if(!knot1_->getAcceleration()->isLocked()) { + + // Construct Jacobian Object + jacs->push_back(Jacobian<>()); + Jacobian<>& jacref = jacs->back(); + jacref.key = knot1_->getAcceleration()->getKey(); + + // Fill in matrix + Eigen::Matrix jacobian; + jacobian.topRows<6>() = -0.5*dt2*Eigen::Matrix::Identity(); + jacobian.block<6,6>(6,0) = -deltaTime*Eigen::Matrix::Identity(); + jacobian.bottomRows<6>() = -Eigen::Matrix::Identity(); + jacref.jac = lhs * jacobian; + + // std::cout << jacobian << std::endl; + } + + // Knot 2 acceleration + if(!knot2_->getAcceleration()->isLocked()) { + + // Construct Jacobian Object + jacs->push_back(Jacobian<>()); + Jacobian<>& jacref = jacs->back(); + jacref.key = knot2_->getAcceleration()->getKey(); + + // Fill in matrix + Eigen::Matrix jacobian; + jacobian.topRows<6>() = Eigen::Matrix::Zero(); + jacobian.block<6,6>(6,0) = Eigen::Matrix::Zero(); + jacobian.bottomRows<6>() = J_21_inv; + jacref.jac = lhs * jacobian; + } + + // Return tree memory to pool + EvalTreeNode::pool.returnObj(evaluationTree1); + EvalTreeNode::pool.returnObj(evaluationTree2); + + // Return error + Eigen::Matrix error; + error.head<6>() = xi_21 - deltaTime*knot1_->getVelocity()->getValue() - 0.5*dt2*knot1_->getAcceleration()->getValue(); + error.segment<6>(6) = J_21_inv*knot2_->getVelocity()->getValue() - knot1_->getVelocity()->getValue() - deltaTime*knot1_->getAcceleration()->getValue(); + error.tail<6>() = -0.5*w*knot2_->getVelocity()->getValue() + J_21_inv*knot2_->getAcceleration()->getValue() - knot1_->getAcceleration()->getValue(); + + return error; +} + +} // se3 +} // steam diff --git a/src/trajectory/SteamCATrajVar.cpp b/src/trajectory/SteamCATrajVar.cpp new file mode 100644 index 0000000..4d692c8 --- /dev/null +++ b/src/trajectory/SteamCATrajVar.cpp @@ -0,0 +1,49 @@ +////////////////////////////////////////////////////////////////////////////////////////////// +/// \file SteamCATrajVar.cpp +/// +/// \author Tim Tang, ASRL +////////////////////////////////////////////////////////////////////////////////////////////// + +#include + +#include + +namespace steam { +namespace se3 { + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +SteamCATrajVar::SteamCATrajVar(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration) + : SteamTrajVar(time, T_k0, velocity), acceleration_(acceleration) { + + // Check velocity input + if (velocity->getPerturbDim() != 6) { + throw std::invalid_argument("invalid velocity size"); + } + + // Check acceleration input + if (acceleration->getPerturbDim() != 6) { + throw std::invalid_argument("invalid acceleration size"); + } +} + +SteamCATrajVar::SteamCATrajVar(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov) + : SteamCATrajVar(time, T_k0, velocity, acceleration) { cov_set_=true; cov_=cov; } + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get acceleration state variable +////////////////////////////////////////////////////////////////////////////////////////////// +const VectorSpaceStateVar::Ptr& SteamCATrajVar::getAcceleration() const { + return acceleration_; +} + +} // se3 +} // steam diff --git a/src/trajectory/SteamTrajInterface.cpp b/src/trajectory/SteamTrajInterface.cpp index 1db4b2c..26714a3 100644 --- a/src/trajectory/SteamTrajInterface.cpp +++ b/src/trajectory/SteamTrajInterface.cpp @@ -27,6 +27,17 @@ SteamTrajInterface::SteamTrajInterface(bool allowExtrapolation) : Qc_inv_(Eigen::Matrix::Identity()), allowExtrapolation_(allowExtrapolation) { } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Constructor +////////////////////////////////////////////////////////////////////////////////////////////// +SteamTrajInterface::SteamTrajInterface(const Eigen::Matrix& Qc_inv, + bool allowExtrapolation) : + Qc_inv_(Qc_inv), allowExtrapolation_(allowExtrapolation) { +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get pose prior cost +////////////////////////////////////////////////////////////////////////////////////////////// double SteamTrajInterface::getPosePriorCost() { if(posePriorFactor_ != nullptr) { return posePriorFactor_->cost(); @@ -35,6 +46,9 @@ double SteamTrajInterface::getPosePriorCost() { } } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get velocity prior cost +////////////////////////////////////////////////////////////////////////////////////////////// double SteamTrajInterface::getVelocityPriorCost() { if(velocityPriorFactor_ != nullptr) { return velocityPriorFactor_->cost(); @@ -42,13 +56,6 @@ double SteamTrajInterface::getVelocityPriorCost() { return 0.0; } } -////////////////////////////////////////////////////////////////////////////////////////////// -/// \brief Constructor -////////////////////////////////////////////////////////////////////////////////////////////// -SteamTrajInterface::SteamTrajInterface(const Eigen::Matrix& Qc_inv, - bool allowExtrapolation) : - Qc_inv_(Qc_inv), allowExtrapolation_(allowExtrapolation) { -} ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Add a new knot @@ -84,20 +91,53 @@ void SteamTrajInterface::add(const steam::Time& time, std::pair(time.nanosecs(), newEntry)); } +void SteamTrajInterface::add(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const Eigen::Matrix cov) { + + // Check velocity input + if (velocity->getPerturbDim() != 6) { + throw std::invalid_argument("invalid velocity size"); + } + + // Todo, check that time does not already exist in map? + + // Make knot + SteamTrajVar::Ptr newEntry(new SteamTrajVar(time, T_k0, velocity, cov)); + + // Insert in map + knotMap_.insert(knotMap_.end(), + std::pair(time.nanosecs(), newEntry)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Add a new knot +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamTrajInterface::add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration) { + add(time, T_k0, velocity); +} + +void SteamTrajInterface::add(const steam::Time& time, const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const VectorSpaceStateVar::Ptr& acceleration, + const Eigen::Matrix cov) { + add(time, T_k0, velocity, cov.topLeftCorner<12,12>()); +} + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get evaluator ////////////////////////////////////////////////////////////////////////////////////////////// TransformEvaluator::ConstPtr SteamTrajInterface::getInterpPoseEval(const steam::Time& time) const { - // Check that map is not empty if (knotMap_.empty()) { throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); } - // Get iterator to first element with time equal to or greater than 'time' std::map::const_iterator it1 = knotMap_.lower_bound(time.nanosecs()); - // Check if time is passed the last entry if (it1 == knotMap_.end()) { @@ -112,7 +152,6 @@ TransformEvaluator::ConstPtr SteamTrajInterface::getInterpPoseEval(const steam:: throw std::runtime_error("Requested trajectory evaluator at an invalid time."); } } - // Check if we requested time exactly if (it1->second->getTime() == time) { @@ -134,14 +173,12 @@ TransformEvaluator::ConstPtr SteamTrajInterface::getInterpPoseEval(const steam:: throw std::runtime_error("Requested trajectory evaluator at an invalid time."); } } - // Get iterators bounding the time interval std::map::const_iterator it2 = it1; --it1; if (time <= it1->second->getTime() || time >= it2->second->getTime()) { throw std::runtime_error("Requested trajectory evaluator at an invalid time. This exception " "should not trigger... report to a STEAM contributor."); } - // Create interpolated evaluator return SteamTrajPoseInterpEval::MakeShared(time, it1->second, it2->second); } @@ -379,6 +416,91 @@ void SteamTrajInterface::appendPriorCostTerms( } } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Store solver in trajectory, needed for querying covariances later +////////////////////////////////////////////////////////////////////////////////////////////// +void SteamTrajInterface::setSolver(std::shared_ptr solver) { + solver_ = solver; +} + +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get interpolated/extrapolated covariance at given time, for now only support at knot times +////////////////////////////////////////////////////////////////////////////////////////////// +Eigen::MatrixXd SteamTrajInterface::getCovariance(const steam::Time& time) const { + + // Check that map is not empty + if (knotMap_.empty()) { + throw std::runtime_error("[GpTrajectory][getEvaluator] map was empty"); + } + + // Get iterator to first element with time equal to or great than 'time' + std::map::const_iterator it1 + = knotMap_.lower_bound(time.nanosecs()); + + // Check if time is passed the last entry + if (it1 == knotMap_.end()) { + --it1; + // If we allow extrapolation + // if (allowExtrapolation_) { + // return extrapCovariance(solver, time); + // } else { + // throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + // } + } + + // Check if we requested time exactly + // if (it1->second->getTime() == time) { + // check if the state variable at time has associated covariacne + if (it1->second->covarianceSet()) { + return it1->second->getCovariance(); + } + // return covariance exactly (no interp) + std::map outState; + it1->second->getPose()->getActiveStateVariables(&outState); + + std::vector keys; + keys.push_back(outState.begin()->second->getKey()); + keys.push_back(it1->second->getVelocity()->getKey()); + + steam::BlockMatrix covariance = solver_->queryCovarianceBlock(keys); + + Eigen::Matrix output; + output.block<6,6>(0,0) = covariance.copyAt(0,0); + output.block<6,6>(0,6) = covariance.copyAt(0,1); + output.block<6,6>(6,0) = covariance.copyAt(1,0); + output.block<6,6>(6,6) = covariance.copyAt(1,1); + return output; + // } + + // // TODO(DAVID): Be able to handle locked states + // // Check if state is locked + // std::map states; + // it1->second->getPose()->getActiveStateVariables(&states); + // if (states.size() == 0) { + // throw std::runtime_error("Attempted covariance interpolation with locked states"); + // } + + // // TODO(DAVID): Extrapolation behind first entry + + // // Check if we requested before first time + // if (it1 == knotMap_.begin()) { + + // // If we allow extrapolation, return constant-velocity interpolated entry + // if (allowExtrapolation_) { + // const SteamTrajVar::Ptr& startKnot = it1->second; + // TransformEvaluator::Ptr T_t_k = + // ConstVelTransformEvaluator::MakeShared(startKnot->getVelocity(), + // time - startKnot->getTime()); + // return compose(T_t_k, startKnot->getPose()); + // } else { + // throw std::runtime_error("Requested trajectory evaluator at an invalid time."); + // } + // } + + + // return interpCovariance(solver, time); +} + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get active state variables in the trajectory ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/src/trajectory/SteamTrajVar.cpp b/src/trajectory/SteamTrajVar.cpp index 950ff86..6c33c18 100644 --- a/src/trajectory/SteamTrajVar.cpp +++ b/src/trajectory/SteamTrajVar.cpp @@ -25,6 +25,12 @@ SteamTrajVar::SteamTrajVar(const steam::Time& time, } } +SteamTrajVar::SteamTrajVar(const steam::Time& time, + const se3::TransformEvaluator::Ptr& T_k0, + const VectorSpaceStateVar::Ptr& velocity, + const Eigen::Matrix cov) + : SteamTrajVar(time, T_k0, velocity) { cov_set_=true; cov_=cov; } + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get pose evaluator ////////////////////////////////////////////////////////////////////////////////////////////// @@ -39,6 +45,21 @@ const VectorSpaceStateVar::Ptr& SteamTrajVar::getVelocity() const { return velocity_; } +////////////////////////////////////////////////////////////////////////////////////////////// +/// \brief Get acceleration state variable, to be overriden +////////////////////////////////////////////////////////////////////////////////////////////// +const VectorSpaceStateVar::Ptr& SteamTrajVar::getAcceleration() const { + throw std::runtime_error("Steam trajectory variable does not have an acceleration state!"); +} + +const Eigen::MatrixXd SteamTrajVar::getCovariance() const { + return cov_; +} + +bool SteamTrajVar::covarianceSet() const { + return cov_set_; +} + ////////////////////////////////////////////////////////////////////////////////////////////// /// \brief Get timestamp ////////////////////////////////////////////////////////////////////////////////////////////// diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 777ee53..4768da0 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -35,6 +35,7 @@ add_executable(steam_unit_tests ${CMAKE_CURRENT_SOURCE_DIR}/sample_test.cpp ${CMAKE_CURRENT_SOURCE_DIR}/time_test.cpp ${CMAKE_CURRENT_SOURCE_DIR}/pattern_test.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/evaluator_test.cpp ) target_link_libraries(steam_unit_tests steam ${DEPEND_LIBS}) diff --git a/tests/evaluator_test.cpp b/tests/evaluator_test.cpp new file mode 100644 index 0000000..44c260c --- /dev/null +++ b/tests/evaluator_test.cpp @@ -0,0 +1,265 @@ +// vim: ts=4:sw=4:noexpandtab + +///////////////////////////////////////////////////////////////////////////////////////////// +/// ErrorEvaluator Test +/// \author Francois Pomerleau +/// \brief To execute only this test run: +/// $ ./tests/steam_unit_tests PointToPointErrorEval -s +///////////////////////////////////////////////////////////////////////////////////////////// + + +#include "catch.hpp" +#include "steam.hpp" +#include + +// Helper function to initialize a 3D point in homogeneous coordinates +Eigen::Vector4d initVector4d(const double x, const double y, const double z) +{ + Eigen::Vector4d v; + v << x, y, z, 1.0; + + return v; +} + +// Helper function to solve and check that the solution is close +// to the ground truth transformation used to generate the data +void solveSimpleProblem(const Eigen::Matrix T_components, const int constructor_type) +{ + //--------------------------------------- + // General structure: + // 0- Generate simple point clouds + // 1- Steam state variables + // 2- Steam cost terms + // 3- Set up the optimization problem + // 4- Solve + // 5- Check that the transformation are the same + //--------------------------------------- + + + //--------------------------------------- + // 0- Generate simple point clouds + //--------------------------------------- + + // Build a fixed point cloud (reference) with 3 points + // expressed in frame a + const Eigen::Vector4d ref_a_0 = initVector4d(0, 0, 0); + const Eigen::Vector4d ref_a_1 = initVector4d(1, 0, 0); + const Eigen::Vector4d ref_a_2 = initVector4d(0, 1, 0); + + // Build a ground truth (gt) transformation matrix + // from frame b to frame a + const lgmath::se3::Transformation Tgt_a_b(T_components); + // and its inverse + const lgmath::se3::Transformation Tgt_b_a = Tgt_a_b.inverse(); + + + // Move the reference point cloud to generate a + // second point cloud called read (reading) + Eigen::Vector4d read_b_0 = Tgt_b_a.matrix() * ref_a_0; + Eigen::Vector4d read_b_1 = Tgt_b_a.matrix() * ref_a_1; + Eigen::Vector4d read_b_2 = Tgt_b_a.matrix() * ref_a_2; + + //--------------------------------------- + // 1- Steam state variables + //--------------------------------------- + + // Setup state for the reference frame + // Note: the default constructor sets the state to identity + steam::se3::TransformStateVar::Ptr stateReference(new steam::se3::TransformStateVar()); + // Lock the reference frame + stateReference->setLock(true); + + // Setup state for the reading frame + steam::se3::TransformStateVar::Ptr stateReading(new steam::se3::TransformStateVar()); + + //--------------------------------------- + // 2- Steam cost terms + //--------------------------------------- + + // steam cost terms + steam::ParallelizedCostTermCollection::Ptr costTerms(new steam::ParallelizedCostTermCollection()); + + // Convert our states to Transform Evaluators + // T_a_a is silly (most be identity) but it's there for completness + auto T_a_a = steam::se3::TransformStateEvaluator::MakeShared(stateReference); + auto T_a_b = steam::se3::TransformStateEvaluator::MakeShared(stateReading); + auto T_b_a = steam::se3::InverseTransformEvaluator::MakeShared(T_a_b); + + // Define our error funtion + typedef steam::PointToPointErrorEval Error; + + //------------- + // This is specific to the unit test + + // Build the alignment errors + Error::Ptr error_0; + Error::Ptr error_1; + Error::Ptr error_2; + + if(constructor_type == 0) + { + error_0.reset(new Error(ref_a_0, T_a_a, read_b_0, T_b_a)); + error_1.reset(new Error(ref_a_1, T_a_a, read_b_1, T_b_a)); + error_2.reset(new Error(ref_a_2, T_a_a, read_b_2, T_b_a)); + } + else if(constructor_type == 1) + { + error_0.reset(new Error(ref_a_0, read_b_0, T_a_b)); + error_1.reset(new Error(ref_a_1, read_b_1, T_a_b)); + error_2.reset(new Error(ref_a_2, read_b_2, T_a_b)); + } + //------------- + + // Set the NoiseModel (R_i) to identity + steam::BaseNoiseModel<4>::Ptr sharedNoiseModel(new steam::StaticNoiseModel<4>(Eigen::Matrix4d::Identity())); + + // Set the LossFunction to L2 (least-squared) + steam::L2LossFunc::Ptr sharedLossFunc(new steam::L2LossFunc()); + + // Define our cost term + typedef steam::WeightedLeastSqCostTerm<4,6> Cost; + + // Build the cost terms + Cost::Ptr cost_0(new Cost(error_0, sharedNoiseModel, sharedLossFunc)); + Cost::Ptr cost_1(new Cost(error_1, sharedNoiseModel, sharedLossFunc)); + Cost::Ptr cost_2(new Cost(error_2, sharedNoiseModel, sharedLossFunc)); + + // Add our individual cost terms to the collection + costTerms->add(cost_0); + costTerms->add(cost_1); + costTerms->add(cost_2); + + + //--------------------------------------- + // 3- Set up the optimization problem + //--------------------------------------- + + steam::OptimizationProblem problem; + + // Add states + problem.addStateVariable(stateReference); + problem.addStateVariable(stateReading); + + // Add cost terms + problem.addCostTerm(costTerms); + + + //--------------------------------------- + // 4- Solve + //--------------------------------------- + + //typedef steam::DoglegGaussNewtonSolver SolverType + typedef steam::VanillaGaussNewtonSolver SolverType; + + SolverType::Params params; + params.verbose = false; + //params.maxIterations = 500; + //params.absoluteCostThreshold = 0.0; + //params.absoluteCostChangeThreshold = 1e-4; + //params.relativeCostChangeThreshold = 1e-4; + + // Make solver + SolverType solver(&problem, params); + + // Optimize + solver.optimize(); + + //--------------------------------------- + // 5- Check that the transformation are the same + //--------------------------------------- + INFO("Minimized transformation:" << "\n" << + stateReading->getValue().matrix() << "\n" << + "is different than original transformation:" << "\n" << + Tgt_b_a.matrix() << "\n" << + "difference being:" << "\n" << + stateReading->getValue().matrix() - Tgt_a_b.matrix() + ); + + // Confirm that our state is the same as our ground truth transformation + CHECK(lgmath::common::nearEqual(stateReading->getValue().matrix(), + Tgt_a_b.matrix(), + 1e-3 + )); + +} + + +TEST_CASE("PointToPointErrorEval", "[ErrorEvaluator]" ) { + + Eigen::Matrix T_components; + + SECTION("Simple translation - constructor 0") + { + T_components << 1.0, // translation x + 0.0, // translation y + 0.0, // translation z + 0.0, // rotation around x-axis + 0.0, // rotation around y-axis + 0.0; // rotation around z-axis + + solveSimpleProblem(T_components, 0); + } + + SECTION("Simple rotation - constructor 0") + { + T_components << 0.0, // translation x + 0.0, // translation y + 0.0, // translation z + 1.0, // rotation around x-axis + 0.0, // rotation around y-axis + 0.0; // rotation around z-axis + + solveSimpleProblem(T_components, 0); + } + + SECTION("Random transformation (1000) - constructor 0") + { + srand((unsigned int) time(0)); + + for(int i=0; i<1000; i++) + { + // random numbers in interval [-1, 1] + T_components = Eigen::Matrix::Random(); + solveSimpleProblem(T_components, 0); + } + + } + + SECTION("Simple translation - constructor 1") + { + T_components << 1.0, // translation x + 0.0, // translation y + 0.0, // translation z + 0.0, // rotation around x-axis + 0.0, // rotation around y-axis + 0.0; // rotation around z-axis + + solveSimpleProblem(T_components, 1); + } + + SECTION("Simple rotation - constructor 1") + { + T_components << 0.0, // translation x + 0.0, // translation y + 0.0, // translation z + 1.0, // rotation around x-axis + 0.0, // rotation around y-axis + 0.0; // rotation around z-axis + + solveSimpleProblem(T_components, 1); + } + + SECTION("Random transformation (1000) - constructor 1") + { + srand((unsigned int) time(0)); + + for(int i=0; i<1000; i++) + { + // random numbers in interval [-1, 1] + T_components = Eigen::Matrix::Random(); + solveSimpleProblem(T_components, 1); + } + + } + +} // TEST_CASE